#include <trajPlanning.h>


vector<vector<double>> TrajPlanning::jointSpacePlan(vector<vector<double>>& anglePosition, vector<double>&time)
{
	vector<vector<double>> jointSpacePlan;
	vector<double> jointOne(6);
	//vector<double> q1_array{ 0,10 }, t1_array{ 0,9 }, v1_array{ 0,0 }, a1_array{ 0,0 }, q2_array{ 0,20 }, q3_array{ 0,30 }, q4_array{ 0,40 }, q5_array{ 0,50 }, q6_array{ 0,60 };
	vector<double> q1_array{ anglePosition[0][0],anglePosition[0][1]},
		q2_array{ anglePosition[1][0],anglePosition[1][1] },
		q3_array{ anglePosition[2][0],anglePosition[2][1] }, q4_array{ anglePosition[3][0],anglePosition[3][1] },
		q5_array{ anglePosition[4][0],anglePosition[4][1] }, q6_array{ anglePosition[5][0],anglePosition[5][1] },
		t1_array=time, v1_array{ 0,0 }, a1_array{ 0,0 } ;
	double t1 = t1_array[0], q1 = q1_array[0], v1 = v1_array[0], a1 = a1_array[0];
	int i = 0;
	double T1 = t1_array[i+1] - t1_array[i];//ÿ�ι滮��ʱ�䣬���ؽ����
	double a01 = q1_array[i], a02 = q2_array[i], a03 = q3_array[i], a04 = q4_array[i], a05 = q5_array[i], a06 = q6_array[i];
	double a11 = v1_array[i], a12 = v1_array[i], a13 = v1_array[i], a14 = v1_array[i], a15 = v1_array[i], a16 = v1_array[i];
	double a21 = a1_array[i]/2, a22 = a1_array[i] / 2, a23 = a1_array[i] / 2, a24 = a1_array[i] / 2, a25 = a1_array[i] / 2, a26 = a1_array[i] / 2;
	double a31 = (20 * q1_array[i + 1] - 20 * q1_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3)),
		   a32= (20 * q2_array[i + 1] - 20 * q2_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3)),
		   a33 = (20 * q3_array[i + 1] - 20 * q3_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3)),
		   a34= (20 * q4_array[i + 1] - 20 * q4_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3)),
		   a35 = (20 * q5_array[i + 1] - 20 * q5_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3)),
		   a36 = (20 * q6_array[i + 1] - 20 * q6_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3));
   double a41 = (30 * q1_array[i] - 30 * q1_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4)),
		   a42= (30 * q2_array[i] - 30 * q2_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4)),
		   a43 = (30 * q3_array[i] - 30 * q3_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4)),
	       a44 = (30 * q4_array[i] - 30 * q4_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4)),
	       a45 = (30 * q5_array[i] - 30 * q5_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4)),
	       a46 = (30 * q6_array[i] - 30 * q6_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4));
	double a51 = (12 * q1_array[i + 1] - 12 * q1_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5)),
		   a52 = (12 * q2_array[i + 1] - 12 * q2_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5)),
		   a53 = (12 * q3_array[i + 1] - 12 * q3_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5)),
		   a54 = (12 * q4_array[i + 1] - 12 * q4_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5)),
	       a55 = (12 * q5_array[i + 1] - 12 * q5_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5)),
		   a56 = (12 * q6_array[i + 1] - 12 * q6_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5));  // ������ζ���ʽϵ��
	int t1_number = (t1_array[i + 1] - t1_array[i]) / 0.01;
	double t_array1[1000] = { 0 }, q_array1[1000] = { 0 }, v_array1[1000] = { 0 }, a_array1[1000] = { 0 },
		t_array2[1000] = { 0 }, q_array2[1000] = { 0 }, v_array2[1000] = { 0 }, a_array2[1000] = { 0 },
		t_array3[1000] = { 0 }, q_array3[1000] = { 0 }, v_array3[1000] = { 0 }, a_array3[1000] = { 0 },
		t_array4[1000] = { 0 }, q_array4[1000] = { 0 }, v_array4[1000] = { 0 }, a_array4[1000] = { 0 },
		t_array5[1000] = { 0 }, q_array5[1000] = { 0 }, v_array5[1000] = { 0 }, a_array5[1000] = { 0 },
		t_array6[1000] = { 0 }, q_array6[1000] = { 0 }, v_array6[1000] = { 0 }, a_array6[1000] = { 0 };
	for (int a1 = 1; a1 <= t1_number; a1 = a1 + 1)
	{
		t_array1[a1]=t_array1[a1-1]+0.01;
		q_array1[a1]= a01 + a11 * (t_array1[a1] - t1_array[i]) + a21 * pow((t_array1[a1] - t1_array[i]),2) + a31 * pow((t_array1[a1] - t1_array[i]), 3) + a41 * pow((t_array1[a1] - t1_array[i]), 4) + a51 * pow((t_array1[a1] - t1_array[i]), 5);
		v_array1[a1]= a11 + 2 * a21 * (t_array1[a1] - t1_array[i]) + 3 * a31 * pow((t_array1[a1] - t1_array[i]), 2) + 4 * a41 * pow((t_array1[a1] - t1_array[i]), 3) + 5 * a51 * pow((t_array1[a1] - t1_array[i]), 4);
		a_array1[a1]= 2 * a21 + 6 * a31 * (t_array1[a1] - t1_array[i]) + 12 * a41 * pow((t_array1[a1] - t1_array[i]), 2) + 20 * a51 * pow((t_array1[a1] - t1_array[i]), 3);//�ؽ�1

		t_array2[a1] = t_array2[a1 - 1] + 0.01;
		q_array2[a1] = a02 + a12 * (t_array2[a1] - t1_array[i]) + a22 * pow((t_array2[a1] - t1_array[i]), 2) + a32 * pow((t_array2[a1] - t1_array[i]), 3) + a42 * pow((t_array2[a1] - t1_array[i]), 4) + a52 * pow((t_array2[a1] - t1_array[i]), 5);
		v_array2[a1] = a12 + 2 * a22 * (t_array2[a1] - t1_array[i]) + 3 * a32 * pow((t_array2[a1] - t1_array[i]), 2) + 4 * a42 * pow((t_array2[a1] - t1_array[i]), 3) + 5 * a52 * pow((t_array2[a1] - t1_array[i]), 4);
		a_array2[a1] = 2 * a22 + 6 * a32 * (t_array2[a1] - t1_array[i]) + 12 * a42 * pow((t_array2[a1] - t1_array[i]), 2) + 20 * a52 * pow((t_array2[a1] - t1_array[i]), 3);

		t_array3[a1] = t_array3[a1 - 1] + 0.01;
		q_array3[a1] = a03 + a13 * (t_array3[a1] - t1_array[i]) + a23 * pow((t_array3[a1] - t1_array[i]), 2) + a33 * pow((t_array3[a1] - t1_array[i]), 3) + a43 * pow((t_array3[a1] - t1_array[i]), 4) + a53 * pow((t_array3[a1] - t1_array[i]), 5);
		v_array3[a1] = a13 + 2 * a23 * (t_array3[a1] - t1_array[i]) + 3 * a33 * pow((t_array3[a1] - t1_array[i]), 2) + 4 * a43 * pow((t_array3[a1] - t1_array[i]), 3) + 5 * a53 * pow((t_array3[a1] - t1_array[i]), 4);
		a_array3[a1] = 2 * a23 + 6 * a33 * (t_array3[a1] - t1_array[i]) + 12 * a43 * pow((t_array3[a1] - t1_array[i]), 2) + 20 * a53 * pow((t_array3[a1] - t1_array[i]), 3);
		
		t_array4[a1] = t_array4[a1 - 1] + 0.01;
		q_array4[a1] = a04 + a14 * (t_array4[a1] - t1_array[i]) + a24 * pow((t_array4[a1] - t1_array[i]), 2) + a34 * pow((t_array4[a1] - t1_array[i]), 3) + a44 * pow((t_array4[a1] - t1_array[i]), 4) + a54 * pow((t_array4[a1] - t1_array[i]), 5);
		v_array4[a1] = a14 + 2 * a24 * (t_array4[a1] - t1_array[i]) + 3 * a34 * pow((t_array4[a1] - t1_array[i]), 2) + 4 * a44 * pow((t_array4[a1] - t1_array[i]), 3) + 5 * a54 * pow((t_array4[a1] - t1_array[i]),4);
		a_array4[a1] = 2 * a24 + 6 * a34 * (t_array4[a1] - t1_array[i]) + 12 * a44 * pow((t_array4[a1] - t1_array[i]), 2) + 20 * a54 * pow((t_array4[a1] - t1_array[i]), 3);

		t_array5[a1] = t_array5[a1 - 1] + 0.01;
		q_array5[a1] = a05 + a15 * (t_array5[a1] - t1_array[i]) + a25 * pow((t_array5[a1] - t1_array[i]), 2) + a35 * pow((t_array5[a1] - t1_array[i]), 3) + a45 * pow((t_array5[a1] - t1_array[i]), 4) + a55 * pow((t_array5[a1] - t1_array[i]), 5);
		v_array5[a1] = a15 + 2 * a25 * (t_array5[a1] - t1_array[i]) + 3 * a35 * pow((t_array5[a1] - t1_array[i]), 2) + 4 * a45 * pow((t_array5[a1] - t1_array[i]), 3) + 5 * a55 * pow((t_array5[a1] - t1_array[i]), 4);
		a_array5[a1] = 2 * a25 + 6 * a35 * (t_array5[a1] - t1_array[i]) + 12 * a45 * pow((t_array5[a1] - t1_array[i]), 2) + 20 * a55 * pow((t_array5[a1] - t1_array[i]), 3);

		t_array6[a1] = t_array6[a1 - 1] + 0.01;
		q_array6[a1] = a06 + a16 * (t_array6[a1] - t1_array[i]) + a26 * pow((t_array6[a1] - t1_array[i]), 2) + a36* pow((t_array6[a1] - t1_array[i]), 3) + a46 * pow((t_array6[a1] - t1_array[i]), 4) + a56 * pow((t_array6[a1] - t1_array[i]), 5);
		v_array6[a1] = a16 + 2 * a26 * (t_array6[a1] - t1_array[i]) + 3 * a36 * pow((t_array6[a1] - t1_array[i]), 2) + 4 * a46 * pow((t_array6[a1] - t1_array[i]), 3) + 5 * a56 * pow((t_array6[a1] - t1_array[i]), 4);
		a_array6[a1] = 2 * a26 + 6 * a36 * (t_array6[a1] - t1_array[i]) + 12 * a46 * pow((t_array6[a1] - t1_array[i]), 2) + 20 * a56 * pow((t_array6[a1] - t1_array[i]), 3);

		jointOne[0] = q_array1[a1];
		jointOne[1] = q_array2[a1];
		jointOne[2] = q_array3[a1];
		jointOne[3] = q_array4[a1];
		jointOne[4] = q_array5[a1];
		jointOne[5] = q_array6[a1];
			/*cout << q_array1[a1] << " "; cout << q_array2[a1] << " "; cout << q_array3[a1] << " "; cout << q_array4[a1] << " "; cout << q_array5[a1] << " ";
			cout << q_array6[a1] << endl;*/
			//q_array1[a1]= a10 + a11 * (ti - t_array(i)) + a21 * (ti - t_array(i)). ^ 2 + a31 * (ti - t_array(i)). ^ 3 + a41 * (ti - t_array(i)). ^ 4 + a51 * (ti - t_array(i)). ^ 5;
		jointSpacePlan.push_back(jointOne);
	}
	
	
	return jointSpacePlan;
}

vector<vector<double>>TrajPlanning::catesianSpacePlan(vector<vector<double>>& position, vector<vector<double>>& posture)
{
	const int numPositionPoints = position.size();

	vector<vector<double>> catesianSpacePosition;
	vector<vector<double>> catesianSpacePosture;
	vector<double>catersianPosition(3);
	vector<double>catersianPosture(3);
	if (numPositionPoints == 2)//采用直线规划
	{
		//对位置进行规划
		

		double d1 = position[1][0] - position[0][0], d2 = position[1][1] - position[0][1], d3 = position[1][2] - position[0][2], d = sqrt(pow(d1, 2) + pow(d2, 2) + pow(d3, 2));//两点间距离
		vector<double> distance{ 0,d }, timePosition{ 0,9 }, velocityPosition{ 0,0 }, acclationPosition{ 0,0 };//定义位移及速度矢量
		vector<double>  P3{ d1 / d,d2 / d,d3 / d };//位移的单位矢量
		//double t1 = timePosition[0], q1 = distance[0], v1 = velocityPosition[0], a1 = acclationPosition[0];
		double interplorationTime = 0.01;
		int i = 0;
		double T1 = timePosition[i + 1] - timePosition[i];//每段规划的时间，各关节相等
		double a01Position = distance[i];
		double a11Position = velocityPosition[i];
		double a21Position = acclationPosition[i] / 2;
		double a31Position = (20 * distance[i + 1] - 20 * distance[i] - (8 * velocityPosition[i + 1] + 12 * velocityPosition[i]) * T1 - (3 * acclationPosition[i] - acclationPosition[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3));
		double a41Position = (30 * distance[i] - 30 * distance[i + 1] + (14 * velocityPosition[i + 1] + 16 * velocityPosition[i]) * T1 + (3 * acclationPosition[i] - 2 * acclationPosition[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4));
		double a51Position = (12 * distance[i + 1] - 12 * distance[i] - (6 * velocityPosition[i + 1] + 6 * velocityPosition[i]) * T1 - (acclationPosition[i] - acclationPosition[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5));
		int t1_number = (timePosition[i + 1] - timePosition[i]) / 0.01;
		double timeInterpolation[1000] = { 0 }, distanceInterpolation[1000] = { 0 }, velocityInterpolation[1000] = { 0 }, acclationInterpolation[1000] = { 0 }, xDistanceInterpolation[1000] = { 0 }, yDistanceInterpolation[1000] = { 0 }, zDistanceInterpolation[1000] = { 0 };
		for (int a1 = 1; a1 <= t1_number; a1 = a1 + 1)
		{
			timeInterpolation[a1] = timeInterpolation[a1 - 1] + 0.01;
			distanceInterpolation[a1] = a01Position + a11Position * (timeInterpolation[a1] - timePosition[i]) + a21Position * pow((timeInterpolation[a1] - timePosition[i]), 2) + a31Position * pow((timeInterpolation[a1] - timePosition[i]), 3) + a41Position * pow((timeInterpolation[a1] - timePosition[i]), 4) + a51Position * pow((timeInterpolation[a1] - timePosition[i]), 5);
			velocityInterpolation[a1] = a11Position + 2 * a21Position * (timeInterpolation[a1] - timePosition[i]) + 3 * a31Position * pow((timeInterpolation[a1] - timePosition[i]), 2) + 4 * a41Position * pow((timeInterpolation[a1] - timePosition[i]), 3) + 5 * a51Position * pow((timeInterpolation[a1] - timePosition[i]), 4);
			acclationInterpolation[a1] = 2 * a21Position + 6 * a31Position * (timeInterpolation[a1] - timePosition[i]) + 12 * a41Position * pow((timeInterpolation[a1] - timePosition[i]), 2) + 20 * a51Position * pow((timeInterpolation[a1] - timePosition[i]), 3);//关节1
			xDistanceInterpolation[a1] = distanceInterpolation[a1] * P3[0]+position[0][0];
			yDistanceInterpolation[a1] = distanceInterpolation[a1] * P3[1]+ position[0][1];
			zDistanceInterpolation[a1] = distanceInterpolation[a1] * P3[2]+ position[0][2];
			catersianPosition[0] = xDistanceInterpolation[a1];
			catersianPosition[1] = yDistanceInterpolation[a1];
			catersianPosition[2] = zDistanceInterpolation[a1];
			catesianSpacePosition.push_back(catersianPosition);
		}
		//对姿态进行规划
		Matrix3d  R1, R2, R;
		R1 << cos(posture[0][2]) * cos(posture[0][1]), cos(posture[0][2])* sin(posture[0][1])* sin(posture[0][0]) - sin(posture[0][2]) * cos(posture[0][0]), cos(posture[0][2])* sin(posture[0][1])* cos(posture[0][0]) + sin(posture[0][2]) * sin(posture[0][0]),
			sin(posture[0][2])* cos(posture[0][1]), sin(posture[0][2])* sin(posture[0][1])* sin(posture[0][0]) + cos(posture[0][2]) * cos(posture[0][0]), sin(posture[0][2])* sin(posture[0][1])* cos(posture[0][0]) - cos(posture[0][2]) * sin(posture[0][0]),
			-sin(posture[0][1]), cos(posture[0][1])* sin(posture[0][0]), cos(posture[0][1])* cos(posture[0][0]);
		R2 << cos(posture[1][2]) * cos(posture[1][1]), cos(posture[1][2])* sin(posture[1][1])* sin(posture[1][0]) - sin(posture[1][2]) * cos(posture[1][0]), cos(posture[1][2])* sin(posture[1][1])* cos(posture[1][0]) + sin(posture[1][2]) * sin(posture[1][0]),
			sin(posture[1][2])* cos(posture[1][1]), sin(posture[1][2])* sin(posture[1][1])* sin(posture[1][0]) + cos(posture[1][2]) * cos(posture[1][0]), sin(posture[1][2])* sin(posture[1][1])* cos(posture[1][0]) - cos(posture[1][2]) * sin(posture[1][0]),
			-sin(posture[1][1]), cos(posture[1][1])* sin(posture[1][0]), cos(posture[1][1])* cos(posture[1][0]);
		// cout << R1 << endl;
		// cout << R2 << endl;
		R = R2 * R1.inverse();//转换矩阵
		double quaternion1_0 = 0.5 * sqrt((1 + R(0, 0) + R(1, 1) + R(2, 2)));
		double quaternion1_1 = (R(2, 1) - R(1, 2)) / (4 * quaternion1_0);
		double quaternion1_2 = (R(0, 2) - R(2, 0)) / (4 * quaternion1_0);
		double quaternion1_3 = (R(1, 0) - R(0, 1)) / (4 * quaternion1_0);//四元数
		double rotaryShaftX = quaternion1_1 / sqrt(quaternion1_1 * quaternion1_1 + quaternion1_2 * quaternion1_2 + quaternion1_3 * quaternion1_3);
		double rotaryShaftY = quaternion1_2 / sqrt(quaternion1_1 * quaternion1_1 + quaternion1_2 * quaternion1_2 + quaternion1_3 * quaternion1_3);
		double rotaryShaftZ = quaternion1_3 / sqrt(quaternion1_1 * quaternion1_1 + quaternion1_2 * quaternion1_2 + quaternion1_3 * quaternion1_3);//四元数对应的旋转轴
		double rotaryAngle = acos(quaternion1_0);
		
		if (quaternion1_0 == 1)
		{
			for (int a1 = 1; a1 <= t1_number; a1 = a1 + 1)
			{
				catersianPosture[0] = posture[0][0];
				catersianPosture[1] = posture[0][1];
				catersianPosture[2] = posture[0][2];
				catesianSpacePosture.push_back(catersianPosture);
			}
		}
		else {
			vector<double> angle{ 0,rotaryAngle }, timePosture1{ 0,9 }, velocityPosture{ 0,0 }, acclationPosture{ 0,0 };//定义位移及速度矢量
			int j = 0;
			double timePosture = timePosture1[j + 1] - timePosture1[j];//每段规划的时间，各关节相等
			double a01Posture = angle[j];
			double a11Posture = velocityPosture[j];
			double a21Posture = acclationPosture[j] / 2;
			double a31Posture = (20 * angle[j + 1] - 20 * angle[j] - (8 * velocityPosture[j + 1] + 12 * velocityPosture[j]) * timePosture - (3 * acclationPosture[j] - acclationPosture[j + 1]) * pow(timePosture, 2)) / (2 * pow(timePosture, 3));
			double a41Posture = (30 * angle[j] - 30 * angle[j + 1] + (14 * velocityPosture[j + 1] + 16 * velocityPosture[j]) * timePosture + (3 * acclationPosture[j] - 2 * acclationPosture[j + 1]) * pow(timePosture, 2)) / (2 * pow(timePosture, 4));
			double a51Posture = (12 * angle[j + 1] - 12 * angle[j] - (6 * velocityPosture[j + 1] + 6 * velocityPosture[j]) * timePosture - (acclationPosture[j] - acclationPosture[j + 1]) * pow(timePosture, 2)) / (2 * pow(timePosture, 5));
			//int t1_number = (timePosture1[j + 1] - timePosture1[j]) / interplorationTime;
			vector<double> t_array1(1000, 0), q_array1(1000, 0), v_array1(1000, 0), a_array1(1000, 0);
			int H = 1000, L = 4;
			int m = 3, n = 3, p = 1000;
			vector<vector<double>> q_1new(H, vector<double>(L, 1));	//vector创建二维数组，并全部初始化为1
			vector< vector < vector<double> > > Req(m, vector< vector<double> >(n, vector<double>(p, 0)));//vector创建三维数组，并全部初始化为0
			vector< vector < vector<double> > > R2end(m, vector< vector<double> >(n, vector<double>(p, 0)));//vector创建三维数组，并全部初始化为0
			vector<vector<double>> RPY2end(1000, vector<double>(3, 0));
			for (int a1 = 1; a1 <= t1_number; a1 = a1 + 1)
			{
				t_array1[a1] = t_array1[a1 - 1] + interplorationTime;
				q_array1[a1] = a01Posture + a11Posture * (t_array1[a1] - timePosture1[j]) + a21Posture * pow((t_array1[a1] - timePosture1[j]), 2) + a31Posture * pow((t_array1[a1] - timePosture1[j]), 3) + a41Posture * pow((t_array1[a1] - timePosture1[j]), 4) + a51Posture * pow((t_array1[a1] - timePosture1[j]), 5);
				v_array1[a1] = a11Posture + 2 * a21Posture * (t_array1[a1] - timePosture1[j]) + 3 * a31Posture * pow((t_array1[a1] - timePosture1[j]), 2) + 4 * a41Posture * pow((t_array1[a1] - timePosture1[j]), 3) + 5 * a51Posture * pow((t_array1[a1] - timePosture1[j]), 4);
				a_array1[a1] = 2 * a21Posture + 6 * a31Posture * (t_array1[a1] - timePosture1[j]) + 12 * a41Posture * pow((t_array1[a1] - timePosture1[j]), 2) + 20 * a51Posture * pow((t_array1[a1] - timePosture1[j]), 3);//关节1
				q_1new[a1][0] = cos(q_array1[a1]);
				q_1new[a1][1] = rotaryShaftX * sin(q_array1[a1]);
				q_1new[a1][2] = rotaryShaftY * sin(q_array1[a1]);
				q_1new[a1][3] = rotaryShaftZ * sin(q_array1[a1]);
				Req[0][0][a1] = pow(q_1new[a1][0], 2) + pow(q_1new[a1][1], 2) - pow(q_1new[a1][2], 2) - pow(q_1new[a1][3], 2);
				Req[0][1][a1] = 2 * (q_1new[a1][1] * q_1new[a1][2] - q_1new[a1][0] * q_1new[a1][3]);
				Req[0][2][a1] = 2 * (q_1new[a1][0] * q_1new[a1][2] + q_1new[a1][1] * q_1new[a1][3]);
				Req[1][0][a1] = 2 * (q_1new[a1][0] * q_1new[a1][3] + q_1new[a1][1] * q_1new[a1][2]);
				Req[1][1][a1] = pow(q_1new[a1][0], 2) - pow(q_1new[a1][1], 2) + pow(q_1new[a1][2], 2) - pow(q_1new[a1][3], 2);
				Req[1][2][a1] = 2 * (q_1new[a1][2] * q_1new[a1][3] - q_1new[a1][0] * q_1new[a1][1]);
				Req[2][0][a1] = 2 * (q_1new[a1][1] * q_1new[a1][3] - q_1new[a1][0] * q_1new[a1][2]);
				Req[2][1][a1] = 2 * (q_1new[a1][0] * q_1new[a1][1] + q_1new[a1][2] * q_1new[a1][3]);
				Req[2][2][a1] = pow(q_1new[a1][0], 2) - pow(q_1new[a1][1], 2) - pow(q_1new[a1][2], 2) + pow(q_1new[a1][3], 2);
				/*cout << Req[0][0][a1] << " "; cout << Req[0][1][a1] << " "; cout << Req[0][2][a1] << endl;
				cout << Req[1][0][a1] << " "; cout << Req[1][1][a1] << " "; cout << Req[1][2][a1] << endl;
				cout << Req[2][0][a1] << " "; cout << Req[2][1][a1] << " "; cout << Req[2][2][a1] << endl; cout <<endl;*/
				Matrix3d  roraionMatrix, R2ed;
				roraionMatrix << Req[0][0][a1], Req[0][1][a1], Req[0][2][a1],
					Req[1][0][a1], Req[1][1][a1], Req[1][2][a1],
					Req[2][0][a1], Req[2][1][a1], Req[2][2][a1];
				R2ed = roraionMatrix * R1;
				if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > -PI / 2 && atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < PI / 2)
				{
					RPY2end[a1][0] = atan2(R2ed(2, 1), R2ed(2, 2)); RPY2end[a1][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[a1][2] = atan2(R2ed(1, 0), R2ed(0, 0));
				}
				if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < -PI / 2 || atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > PI / 2)
				{
					RPY2end[a1][0] = atan2(-R2ed(2, 1), -R2ed(2, 2)); RPY2end[a1][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[a1][2] = atan2(-R2ed(1, 0), -R2ed(0, 0));
				}
				//cout << RPY2end[a1][0] << " "; cout << RPY2end[a1][1] << " "; cout << RPY2end[a1][2] << endl;
				catersianPosture[0] = RPY2end[a1][0];
				catersianPosture[1] = RPY2end[a1][1];
				catersianPosture[2] = RPY2end[a1][2];
				catesianSpacePosture.push_back(catersianPosture);
			}
		}
		vector<vector<double>>catesianSpacePlan(t1_number, vector<double>(6, 0));
		for (int a1 = 0; a1 <= t1_number-1; a1 = a1 + 1)
		{
			catesianSpacePlan[a1][0] = catesianSpacePosition[a1][0];
			catesianSpacePlan[a1][1] = catesianSpacePosition[a1][1];
			catesianSpacePlan[a1][2] = catesianSpacePosition[a1][2];
			catesianSpacePlan[a1][3] = catesianSpacePosture[a1][0];
			catesianSpacePlan[a1][4] = catesianSpacePosture[a1][1];
			catesianSpacePlan[a1][5] = catesianSpacePosture[a1][2];
		}
		return catesianSpacePlan;
	}
//	if (numPositionPoints == 3) 
//	{
//		//轨迹圆弧规划
//		double interplorationTime = 0.02;
//		double x1 = position[0][0], y1 = position[0][1], z1 = position[0][2],
//			x2 = position[1][0], y2 = position[1][1], z2 = position[1][2],
//			x3 = position[2][0], y3 = position[2][1], z3 = position[2][2];//可以输入的量
//		vector<double> P1{ x1 ,y1,z1 }, P2{ x2,y2,z2 }, P3{ x3,y3,z3 };//空间中三点位置
//		double m11 = (y2 - y1) * (z3 - z2) - (y3 - y2) * (z2 - z1), m12 = (x3 - x2) * (z2 - z1) - (x2 - x1) * (z3 - z2),
//			m13 = (x2 - x1) * (y3 - y2) - (x3 - x2) * (y2 - y1), m14 = m11 * x1 + m12 * y1 + m13 * z1;//三点所在平面的系数
//		double m21 = x2 - x1, m22 = y2 - y1, m23 = z2 - z1,
//			m24 = (pow(x2, 2) - pow(x1, 2) + pow(y2, 2) - pow(y1, 2) + pow(z2, 2) - pow(z1, 2)) / 2;//1 2两点的垂直平分面
//		double m31 = x3 - x2, m32 = y3 - y2, m33 = z3 - z2,
//			m34 = (pow(x3, 2) - pow(x2, 2) + pow(y3, 2) - pow(y2, 2) + pow(z3, 2) - pow(z2, 2)) / 2;//2 3两点的垂直平分面
//		Matrix3d  M1;
//		MatrixXd M2(3, 1), P(3, 1);
//		Eigen::Vector3f x_1;
//		Eigen::Vector3f y_1;
//		Eigen::Vector3f z_1;
//		M1 << m11, m12, m13,
//			m21, m22, m23,
//			m31, m32, m33;
//		M2 << m14, m24, m34;
//		P = M1.inverse() * M2;
//		double r = sqrt(pow(x1 - P(0), 2) + pow(y1 - P(1), 2) + pow(z1 - P(2), 2));//半径
//		//建立新的坐标系
//		double m1 = sqrt(pow(m11, 2) + pow(m12, 2) + pow(m13, 2));
//		z_1 << m11 / m1, m12 / m1, m13 / m1;
//		x_1 << pow(x1 - P(0), 1) / r, pow(y1 - P(1), 1) / r, pow(z1 - P(2), 1) / r;
//		y_1 << z_1.cross(x_1);
//		MatrixXd x_11(3, 1), y_11(3, 1), z_11(3, 1);
//		x_11 << x_1(0), x_1(1), x_1(2);
//		y_11 << y_1(0), y_1(1), y_1(2);
//		z_11 << z_1(0), z_1(1), z_1(2);
//		Matrix3d  R01;
//		R01 << x_11, y_11, z_11;
//		Matrix4d  T01;
//		T01 = RotaToHomo(R01, P);//齐次转换矩阵
//		MatrixXd P1_1(4, 1), P2_1(4, 1), P3_1(4, 1), P11(4, 1), P21(4, 1), P31(4, 1);//三个原始点的齐次坐标
//		P1_1 << P1[0], P1[1], P1[2], 1;
//		P2_1 << P2[0], P2[1], P2[2], 1;
//		P3_1 << P3[0], P3[1], P3[2], 1;
//		P11 = T01.inverse() * P1_1;
//		P21 = T01.inverse() * P2_1;
//		P31 = T01.inverse() * P3_1;//三个原始点的在新坐标系中的位置
//		double theta21 = 0;
//		double theta31 = 0;
//		double theta = 0;
//		if (atan2(P21(1), P21(0)) < 0)
//		{
//			theta21 = atan2(P21(1), P21(0)) + 2 * PI;
//		}
//		else
//		{
//			theta21 = atan2(P21(1), P21(0));
//		}
//		if (atan2(P31(1), P31(0)) < 0)
//		{
//			theta31 = atan2(P31(1), P31(0)) + 2 * PI;
//		}
//		else
//		{
//			theta31 = atan2(P31(1), P31(0));
//		}
//		if (theta31 - theta21 > 0)
//		{
//			theta = theta31;
//		}
//		else
//		{
//			theta = 2 * PI - theta31;
//		}
//		vector<double> q1_array{ 0,theta }, t1_array{ 0,9 }, v1_array{ 0,0 }, a1_array{ 0,0 };//定义位移及速度矢量
//		double t1 = t1_array[0], q1 = q1_array[0], v1 = v1_array[0], a1 = a1_array[0];
//		int i = 0;
//		double T1 = t1_array[i + 1] - t1_array[i];//每段规划的时间，各关节相等
//		double a01 = q1_array[i];
//		double a11 = v1_array[i];
//		double a21 = a1_array[i] / 2;
//		double a31 = (20 * q1_array[i + 1] - 20 * q1_array[i] - (8 * v1_array[i + 1] + 12 * v1_array[i]) * T1 - (3 * a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 3));
//		double a41 = (30 * q1_array[i] - 30 * q1_array[i + 1] + (14 * v1_array[i + 1] + 16 * v1_array[i]) * T1 + (3 * a1_array[i] - 2 * a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 4));
//		double a51 = (12 * q1_array[i + 1] - 12 * q1_array[i] - (6 * v1_array[i + 1] + 6 * v1_array[i]) * T1 - (a1_array[i] - a1_array[i + 1]) * pow(T1, 2)) / (2 * pow(T1, 5));
//		int t1_number = (t1_array[i + 1] - t1_array[i]) / interplorationTime;
//		double t_array1[2000] = { 0 }, theta_array1[1000] = { 0 }, v_array1[1000] = { 0 }, a_array1[1000] = { 0 }, Px_new[1000] = { 0 }, Py_new[1000] = { 0 }, Pz_new[1000] = { 0 }, Px_wrold[1000] = { 0 }, Py_wrold[1000] = { 0 }, Pz_wrold[1000] = { 0 };
//		//Eigen::Vector4f P_new;
//		//Eigen::Vector4f P_wrold;
//		MatrixXd P_new(4, 1), P_wrold(4, 1);
//		for (int a1 = 1; a1 <= t1_number; a1 = a1 + 1)
//		{
//			t_array1[a1] = t_array1[a1 - 1] + interplorationTime;
//			theta_array1[a1] = a01 + a11 * (t_array1[a1] - t1_array[i]) + a21 * pow((t_array1[a1] - t1_array[i]), 2) + a31 * pow((t_array1[a1] - t1_array[i]), 3) + a41 * pow((t_array1[a1] - t1_array[i]), 4) + a51 * pow((t_array1[a1] - t1_array[i]), 5);
//			v_array1[a1] = a11 + 2 * a21 * (t_array1[a1] - t1_array[i]) + 3 * a31 * pow((t_array1[a1] - t1_array[i]), 2) + 4 * a41 * pow((t_array1[a1] - t1_array[i]), 3) + 5 * a51 * pow((t_array1[a1] - t1_array[i]), 4);
//			a_array1[a1] = 2 * a21 + 6 * a31 * (t_array1[a1] - t1_array[i]) + 12 * a41 * pow((t_array1[a1] - t1_array[i]), 2) + 20 * a51 * pow((t_array1[a1] - t1_array[i]), 3);//关节1
//			Px_new[a1] = r * cos(theta_array1[a1]);
//			Py_new[a1] = r * sin(theta_array1[a1]);
//			Pz_new[a1] = 0;
//			P_new << Px_new[a1], Py_new[a1], Pz_new[a1], 1;
//			P_wrold = T01 * P_new;
//			/*Px_wrold[a1] = P_wrold(0);
//			Py_wrold[a1] = P_wrold(1);
//			Pz_wrold[a1] = P_wrold(2);*/
//			catersianPosition[0] = P_wrold(0);
//			catersianPosition[1] = P_wrold(1);
//			catersianPosition[2] = P_wrold(2);
//			catesianSpacePosition.push_back(catersianPosition);
//		}
//		//姿态规划
//		Matrix3d  R1, Rt1, Rt2;
//		R1 << cos(posture[0][2]) * cos(posture[0][1]), cos(posture[0][2])* sin(posture[0][1])* sin(posture[0][0]) - sin(posture[0][2]) * cos(posture[0][0]), cos(posture[0][2])* sin(posture[0][1])* cos(posture[0][0]) + sin(posture[0][2]) * sin(posture[0][0]),
//			sin(posture[0][2])* cos(posture[0][1]), sin(posture[0][2])* sin(posture[0][1])* sin(posture[0][0]) + cos(posture[0][2]) * cos(posture[0][0]), sin(posture[0][2])* sin(posture[0][1])* cos(posture[0][0]) - cos(posture[0][2]) * sin(posture[0][0]),
//			-sin(posture[0][1]), cos(posture[0][1])* sin(posture[0][0]), cos(posture[0][1])* cos(posture[0][0]);
//		double q_points[] = { 1, 0, 0,
//			   (2 + sqrt(2)) / 4, sqrt(2) / 4, 0,
//			   sqrt(2) * 0.5, sqrt(2) * 0.5, 0,
//			   (1 + sqrt(2)) / 4, (1 + sqrt(2)) / 4,sqrt(2) / 4,
//				0.5, 0.5, sqrt(2)/2 };
//		const int num_points = sizeof(q_points) / sizeof(q_points[0]) / 3;///                                                                                    需要根据数据个数调整
//		const double cstartpar = 0;
//		int type[num_points] = { 0 };
//		for (int i = 0; i <= num_points - 1; i++)
//		{
//			type[i] = 1;
//		}//需要根据数据个数调整
//		int ik = 4;
//		/*double n_control = size(u) - p - 2;*/
//		double cendpar;
//		SISLCurve* result_curve = 0;
//		double* gpar = 0;
//		int jnbpar;
//		int jstat = 0;
//		s1356(q_points,        // pointer to where the point coordinates are stored
//			num_points,    // number of points to be interpolated
//			3,             // the dimension
//			type,          // what type of information is stored at a particular point
//			0,             // no additional condition at start point
//			0,             // no additional condition at end point
//			1,             // open curve
//			4,             // order of the spline curve to be produced
//			cstartpar,     // parameter value to be used at start of curve
//			&cendpar,      // parameter value at the end of the curve (to be determined)
//			&result_curve, // the resulting spline curve (to be determined)
//			&gpar,         // pointer to the parameter values of the points in the curve
//			// (to be determined)
//			&jnbpar,       // number of unique parameter values (to be determined)
//			&jstat);       // status message
//		if (jstat < 0) {
//			throw runtime_error("Error occured inside call to SISL routine.");
//		}
//		else if (jstat > 0) {
//			cerr << "WARNING: warning occured inside call to SISL routine. \n" << endl;
//		}
//		//cout << "Total parameter interval of curve: [" << cstartpar << ", "
//			//<< cendpar << "]\n\n";
//		//cout << "Point parameter values was decided to be: \n";
//		for (int i = 0; i < num_points; ++i)
//		{
//			//cout << gpar[i] << ' ';
//		}
//		//cout << endl;
//		//cout << jnbpar << endl;
//		double u_start = 0, u_end = gpar[num_points - 1];
//		double segmentationnumber = t1_number;//分割步数
//		double h = (u_end - u_start) / segmentationnumber;
//		//cout << h << endl;
//		vector<double> u_1(1000, 0);
//		double derive[9] = { 0 };/* Curve dimension times (der + 1) */
//		int m = 1000, n = 9;
//		vector < vector<double> > cruve_der(m, vector<double>(n, 1));//vector创建二维数组，曲线上的点 一阶导 二阶导
//		double s_sum = 0, sum_1 = 0, sum_2 = 0;
//		int der = 2;
//		int UKnotspan = { 0 };
//		double curvature[3] = { 0 }; /* Curve dimension */
//		double radius_of_curvature = 0;
//		vector<vector<double>> cruvePoint(segmentationnumber + 1, vector<double>(3, 0));//插值点在单位球面上的投影
//		for (int a1 = 0; a1 <= segmentationnumber; a1++)
//		{
//			u_1[a1 + 1] = u_1[a1] + h;
//			double parvalue = u_1[a1];
//			s1225(result_curve, der, parvalue, &UKnotspan, derive, curvature, &radius_of_curvature, &jstat);
//			/*cout << derive[0] << ' ';
//			cout << derive[1] << ' ';
//			cout << derive[2] << endl;
//			cout << cendpar*derive[3]<< endl;*/
//			/*cout << cendpar * derive[4] << endl;
//			cout << cendpar * derive[5] << endl;*/
//			//cout << endl;
//			cruve_der[a1][0] = derive[0];
//			cruve_der[a1][1] = derive[1];
//			cruve_der[a1][2] = derive[2];
//			cruve_der[a1][3] = derive[3];
//			cruve_der[a1][4] = derive[4];
//			cruve_der[a1][5] = derive[5];
//			cruve_der[a1][6] = derive[6];
//			cruve_der[a1][7] = derive[7];
//			cruve_der[a1][8] = derive[8];
//			cruvePoint[a1][0] = derive[0] / sqrt(pow(derive[0], 2) + pow(derive[1], 2) + pow(derive[2], 2));
//			cruvePoint[a1][1] = derive[1] / sqrt(pow(derive[0], 2) + pow(derive[1], 2) + pow(derive[2], 2));
//			cruvePoint[a1][2] = derive[2] / sqrt(pow(derive[0], 2) + pow(derive[1], 2) + pow(derive[2], 2));
//		}
//		
//		vector< vector < vector<double> > > Req(3, vector< vector<double> >(3, vector<double>( segmentationnumber + 1, 0)));//vector创建三维数组，并全部初始化为0
//		vector<vector<double>> quaternions( segmentationnumber + 1, vector<double>(4, 0));//在插值过程中的四元数
//		vector<vector<double>> RPY2end(1000, vector<double>(3, 0));
//		for (int i = 0; i <= segmentationnumber; i++)
//		{
//			quaternions[i][0] = cruvePoint[0][0] * cruvePoint[i][0] + cruvePoint[0][1] * cruvePoint[i][1] + cruvePoint[0][2] * cruvePoint[i][2];
//			quaternions[i][1] = cruvePoint[0][1] * cruvePoint[i][2] - cruvePoint[0][2] * cruvePoint[i][1];
//			quaternions[i][2] = cruvePoint[0][2] * cruvePoint[i][0] - cruvePoint[0][0] * cruvePoint[i][2];
//			quaternions[i][3] = cruvePoint[0][0] * cruvePoint[i][1] - cruvePoint[0][1] * cruvePoint[i][0];
//			//cout << quaternions[i][0] << " "; cout << quaternions[i][1] << " "; cout << quaternions[i][2] << " "; cout << quaternions[i][3] << endl;
//			Req[0][0][i] = pow(quaternions[i][0], 2) + pow(quaternions[i][1], 2) - pow(quaternions[i][2], 2) - pow(quaternions[i][3], 2);
//			Req[0][1][i] = 2 * (quaternions[i][1] * quaternions[i][2] - quaternions[i][0] * quaternions[i][3]);
//			Req[0][2][i] = 2 * (quaternions[i][0] * quaternions[i][2] + quaternions[i][1] * quaternions[i][3]);
//			Req[1][0][i] = 2 * (quaternions[i][0] * quaternions[i][3] + quaternions[i][1] * quaternions[i][2]);
//			Req[1][1][i] = pow(quaternions[i][0], 2) - pow(quaternions[i][1], 2) + pow(quaternions[i][2], 2) - pow(quaternions[i][3], 2);
//			Req[1][2][i] = 2 * (quaternions[i][2] * quaternions[i][3] - quaternions[i][0] * quaternions[i][1]);
//			Req[2][0][i] = 2 * (quaternions[i][1] * quaternions[i][3] - quaternions[i][0] * quaternions[i][2]);
//			Req[2][1][i] = 2 * (quaternions[i][0] * quaternions[i][1] + quaternions[i][2] * quaternions[i][3]);
//			Req[2][2][i] = pow(quaternions[i][0], 2) - pow(quaternions[i][1], 2) - pow(quaternions[i][2], 2) + pow(quaternions[i][3], 2);
//			Matrix3d  roraionMatrix, R2ed;
//			roraionMatrix << Req[0][0][i], Req[0][1][i], Req[0][2][i],
//				Req[1][0][i], Req[1][1][i], Req[1][2][i],
//				Req[2][0][i], Req[2][1][i], Req[2][2][i];
//			R2ed = roraionMatrix * R1;
//			if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > -PI / 2 && atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < PI / 2)
//			{
//				RPY2end[i][0] = atan2(R2ed(2, 1), R2ed(2, 2)); RPY2end[i][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[i][2] = atan2(R2ed(1, 0), R2ed(0, 0));
//			}
//			if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < -PI / 2 || atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > PI / 2)
//			{
//				RPY2end[i][0] = atan2(-R2ed(2, 1), -R2ed(2, 2)); RPY2end[i][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[i][2] = atan2(-R2ed(1, 0), -R2ed(0, 0));
//			}
//			//cout << RPY2end[i][0] << " "; cout << RPY2end[i][1] << " "; cout << RPY2end[i][2] << endl;
//			catersianPosture[0] = RPY2end[a1][0];
//			catersianPosture[1] = RPY2end[a1][1];
//			catersianPosture[2] = RPY2end[a1][2];
//			catesianSpacePosture.push_back(catersianPosture);
//		}
//		vector<vector<double>>catesianSpacePlan(t1_number, vector<double>(6, 0));
//		for (int a1 = 1; a1 <= t1_number; a1 = a1 + 1)
//		{
//			catesianSpacePlan[a1][0] = catesianSpacePosition[a1][0];
//			catesianSpacePlan[a1][1] = catesianSpacePosition[a1][1];
//			catesianSpacePlan[a1][2] = catesianSpacePosition[a1][2];
//			catesianSpacePlan[a1][3] = catesianSpacePosture[a1][0];
//			catesianSpacePlan[a1][4] = catesianSpacePosture[a1][1];
//			catesianSpacePlan[a1][5] = catesianSpacePosture[a1][2];
//		}
//		return catesianSpacePlan;
//	}
	if (numPositionPoints > 3) 
	{
		//对位置进行规划
		/*double q_points[] = { ,
		605.95,520, 5,
		610.95,510, 10,
		612.95,500, 13,
		614.95,498, 19,
		618.95,496, 25,
		620.95,495, 28,
		625.95,492, 30 };*/
		int positionRow = position.size();//行数
		int positionColumn = position[0].size();//列数
		/*double **arr2=new int*[]*/
		double* q_points;
		q_points = (double*)malloc(sizeof(double)*positionRow * positionColumn);
		if (q_points == NULL) {
		//	cout << "分配失败" << endl;
		}
		int index = 0;
		for (int j = 0; j < positionRow; ++j) {
			for (int k = 0; k < positionColumn; ++k) {
				*(q_points + index) = position[j][k];
				++index;
			}
		}
		for (int i = 0; i < positionRow; ++i) {
			for (int j = 0; j < positionColumn; ++j) {

				//cout << position[i][j] << " ";
			}
		}
		//cout << endl;
		for (int i = 0; i < positionRow * positionColumn; ++i) {
			//cout << *(q_points + i)<<" ";
		}
		const double cstartpar = 0;
		int* type;
		type = (int*)malloc(sizeof(int) * positionRow);
		for (int i = 0; i < positionRow; i++)
		{
			*(type+i) = 1;
		}
		
		//double q[5][2] = { {0,0}, {3,4}, {-1,4},{-4,0},{-4,-3} };
		int ik = 4;
		/*double n_control = size(u) - p - 2;*/
		double cendpar;
		SISLCurve* result_curve = 0;
		double* gpar = 0;
		int jnbpar;
		int jstat = 0;
		s1356(q_points,        // pointer to where the point coordinates are stored
			positionRow,    // number of points to be interpolated
			3,             // the dimension
			type,          // what type of information is stored at a particular point
			0,             // no additional condition at start point
			0,             // no additional condition at end point
			1,             // open curve
			4,             // order of the spline curve to be produced
			cstartpar,     // parameter value to be used at start of curve
			&cendpar,      // parameter value at the end of the curve (to be determined)
			&result_curve, // the resulting spline curve (to be determined)
			&gpar,         // pointer to the parameter values of the points in the curve
			// (to be determined)
			&jnbpar,       // number of unique parameter values (to be determined)
			&jstat);       // status message

		////释放
		free(q_points);
		free(type);

		if (jstat < 0) {
			throw runtime_error("Error occured inside call to SISL routine.");
		}
		else if (jstat > 0) {
			cerr << "WARNING: warning occured inside call to SISL routine. \n" << endl;
		}

		//cout << "Total parameter interval of curve: [" << cstartpar << ", "
			//<< cendpar << "]\n\n";
		//cout << "Point parameter values was decided to be: \n";
		for (int i = 0; i < positionRow; ++i)
		{
			//cout << gpar[i] << ' ';
		}
		//cout << endl;
		//cout << jnbpar << endl;
		
		//辛普森求弧长
		double u_start = 0, u_end = gpar[positionRow - 1];
		double segmentationnumber = 200;//分割步数//修改原200
		double h = (u_end - u_start) / segmentationnumber;
		//cout << h << endl;
		vector<double> u_1(1000, 0), y_1(1000, 0);
		double derive[9] = { 0 };/* Curve dimension times (der + 1) */
		int m = 1000, n = 9;
		vector < vector<double> > cruve_der(m, vector<double>(n, 1));//vector创建二维数组，曲线上的点 一阶导 二阶导
		double s_sum = 0, sum_1 = 0, sum_2 = 0;
		int der = 2;
		int UKnotspan = { 0 };
		double curvature[3] = { 0 }; /* Curve dimension */
		double radius_of_curvature = 0;
		// ofstream input("F:\\机器人学\\五院\\存入数据.txt");//存入数据名
		for (int a1 = 0; a1 <= 2 * segmentationnumber; a1++)
		{
			u_1[a1 + 1] = u_1[a1] + h * 0.5;
			double parvalue = u_1[a1];
			s1225(result_curve, der, parvalue, &UKnotspan, derive, curvature, &radius_of_curvature, &jstat);
			/*cout << derive[0] << ' ';
			cout << derive[1] << ' ';
			cout << 17*derive[2] << ' ';
			cout << 17*derive[3] << endl;*/
			cruve_der[a1][0] = derive[0];
			cruve_der[a1][1] = derive[1];
			cruve_der[a1][2] = derive[2];
			cruve_der[a1][3] = derive[3];
			cruve_der[a1][4] = derive[4];
			cruve_der[a1][5] = derive[5];
			cruve_der[a1][6] = derive[6];
			cruve_der[a1][7] = derive[7];
			cruve_der[a1][8] = derive[8];
			for (int i = 0; i < 3; i++)//行
			{
				// input << derive[i] << " ";
				//
			}
			// input << endl;
			y_1[a1] = sqrt(pow(cruve_der[a1][3], 2) + pow(cruve_der[a1][4], 2) + pow(cruve_der[a1][5], 2));//曲线上点的函数值
		}
		// input.close();

		for (int a2 = 1; a2 <= 2 * segmentationnumber - 1; a2 = a2 + 2)//偶数部分函数值
		{
			sum_1 = sum_1 + y_1[a2];
		}
		for (int a3 = 2; a3 <= 2 * segmentationnumber - 2; a3 = a3 + 2)//奇数部分函数值
		{
			sum_2 = sum_2 + y_1[a3];
		}
		s_sum = h / 6 * (y_1[0] + y_1[2 * segmentationnumber]) + 2 * h * sum_1 / 3 + h * sum_2 / 3;//整段计算弧长
		// cout << "曲线总长： " << s_sum << endl;

		//计算曲率极值点
		Eigen::Vector3f der_1;//一阶导
		Eigen::Vector3f der_2;//二阶导
		Eigen::Vector3f numerator_vector;
		vector<double> numerator_1(1000, 0), denominator_1(1000, 0), curvatureFind(1000, 0);
		vector<double> curvaturemax(100, 0), curvaturemaxposition(100, 0);
		double maxpointnumber = 0;
		for (int k1 = 0; k1 <= segmentationnumber; k1++)
		{
			der_1 << cruve_der[2 * k1][3], cruve_der[2 * k1][4], cruve_der[2 * k1][5];
			der_2 << cruve_der[2 * k1][6], cruve_der[2 * k1][7], cruve_der[2 * k1][8];
			numerator_vector = der_1.cross(der_2);
			numerator_1[k1] = sqrt(pow(numerator_vector(0), 2) + pow(numerator_vector(1), 2) + pow(numerator_vector(2), 2)); //曲率分子
			denominator_1[k1] = sqrt(pow(cruve_der[2 * k1][3], 2) + pow(cruve_der[2 * k1][4], 2) + pow(cruve_der[2 * k1][5], 2)); // 曲率分母 曲率关于u导数
			curvatureFind[k1] = numerator_1[k1] / pow(denominator_1[k1], 3); // 求曲率	
			//cout << curvatureFind[k1] << endl;
		}

		double maxCurvatureNumber = 0;
		for (int k2 = 2; k2 <= segmentationnumber - 1; k2++)
		{
			if (curvatureFind[k2] >= curvatureFind[k2 - 1] && curvatureFind[k2] >= curvatureFind[k2 + 1])

			{
					curvaturemax[maxpointnumber] = curvatureFind[k2];
					curvaturemaxposition[maxpointnumber] = k2;//曲率最大值的位置数字
					maxpointnumber = maxpointnumber + 1;
				
			}
		}
		// cout << "曲率极大值点的个数：  " << maxpointnumber << endl;
		////起始点以及曲率极大值点对应的u
		vector<double> segmentPointu(maxpointnumber + 2, 0);
		segmentPointu[0] = 0; segmentPointu[maxpointnumber + 1] = 1;
		for (int maxpoint = 1; maxpoint <= maxpointnumber; maxpoint++)
		{
			segmentPointu[maxpoint] = (curvaturemaxposition[maxpoint - 1]) / segmentationnumber; //储存曲率极值点的位置		
		}
		//cout << segmentPointu[0] << ' ';
		//cout << segmentPointu[1] << ' ';
		//cout << segmentPointu[2] << ' ';
		//cout << segmentPointu[3] << endl;
		
		vector<double> segmentlength(1000, 0), segmentLengthSum(1000, 0), u(1000, 0), uSum(1000, 0);
		for (int point = 0; point <= 2 * segmentationnumber - 2; point = point + 2)
		{
			segmentlength[point / 2] = h / 6 * (y_1[point] + 4 * y_1[point + 1] + y_1[point + 2]); //	求出每小段弧长度
			segmentLengthSum[point / 2 + 1] = segmentLengthSum[point / 2] + segmentlength[point / 2];//前point/2段的总长
			u[point / 2] = h;
			uSum[point / 2 + 1] = uSum[point / 2] + u[point / 2];//前point/2段的u
		}
		// cout << "前xx段曲线总长： " << segmentLengthSum[1] << endl;
		//cout << endl;
		//cout << "前xx段曲线的总u： " << uSum[3] << endl;
		//计算分割后每两个极值点间的弧长
		vector<double>  curvaturemaxlength(maxpointnumber + 2, 0), everyLength(maxpointnumber + 1, 0);
		for (int maxPointNumber = 0; maxPointNumber <= maxpointnumber + 1; maxPointNumber++)
		{
			curvaturemaxlength[maxPointNumber] = segmentLengthSum[segmentPointu[maxPointNumber] * segmentationnumber];
		}
		//cout << "前xx段曲线的总长（曲率极大值点分界）：" << curvaturemaxlength[3] << endl;

		for (int everyLengthNumber = 0; everyLengthNumber <= maxpointnumber; everyLengthNumber++)
		{
			everyLength[everyLengthNumber] = curvaturemaxlength[everyLengthNumber + 1] - curvaturemaxlength[everyLengthNumber];//计算出每段的弧长
		}
		// cout << "第xx段曲线的长度（两曲率极大值之间）" << everyLength[0] << endl;
		// cout << "第xx段曲线的长度（两曲率极大值之间）" << everyLength[1] << endl;
		// cout << "第xx段曲线的长度（两曲率极大值之间）" << everyLength[2] << endl;

		////s型速度曲线规划
		//第一段
		double vmax = 5, vlimit11 =2, vlimit12=2,amax =2, jmax = 2;
		double t1 = amax / jmax;
		double t2 = (vmax - vlimit11) / amax;
		double t3 = t2 + t1, t4 = t3 + 0;
		double t5 = t4 + t1, t6 = t5 + ((vmax - vlimit12)/amax - t1), t7 = t6 + t1;
		double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
		double s2 = vlimit11 * (t2-t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
		double s3 = vmax * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - vmax * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
		double s4 = vmax * (t4 - t3);
		double s5 = vmax * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - vmax * t4 + jmax * pow(t4, 3) / 6;
		double s6 = vlimit12 * (t6-t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
		double s7 = vlimit12 * (t7-t6)+ 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
		double sa1 = s1+s2+s3+s4+s5+s6+s7; // 计算出不包含匀速段的总位移
		// cout << sa1 << endl;
		//各段长度相等
		
			//计算新的时间与最大速度
		int segmenttimeRow = everyLength.size(), segmenttimeColumn = 7;
		//cout << segmenttimeRow << endl;
		vector<vector<double>> segmentTime(segmenttimeRow, vector<double>(segmenttimeColumn, 0));
		vector<vector<double>> lengthNew(segmenttimeRow, vector<double>(segmenttimeColumn, 0));
		vector<vector<double>> lengthNewSum(segmenttimeRow, vector<double>(segmenttimeColumn, 0));//极值点之间的弧长 S速度规划的路径和
		vector<double> ssum(segmenttimeRow, 0);
		vector<double> segmentVmax(everyLength.size(), vmax);
		//中间段
		for (int segmentnumber = 1; segmentnumber <= everyLength.size() - 2; segmentnumber++)
		{
			if (everyLength[segmentnumber] >= sa1)
			{
				double deltatime34 = (everyLength[segmentnumber] - sa1) / vmax;
				double t1 = amax / jmax;
				double t2 = (vmax - vlimit11) / amax;
				double t3 = t2 + t1, t4 = t3 + deltatime34;
				double t5 = t4 + t1, t6 = t5 + ((vmax - vlimit12) / amax - t1), t7 = t6 + t1;
				double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
				double s2 = vlimit11 * (t2-t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
				double s3 = vmax * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - vmax * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
				double s4 = vmax * (t4 - t3);
				double s5 = vmax * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - vmax * t4 + jmax * pow(t4, 3) / 6;
				double s6 = vlimit12 * (t6-t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
				double s7 = vlimit12 * (t7-t6) + 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
				segmentTime[segmentnumber][0] = t1; segmentTime[segmentnumber][1] = t2; segmentTime[segmentnumber][2] = t3;
				segmentTime[segmentnumber][3] = t4; segmentTime[segmentnumber][4] = t5; segmentTime[segmentnumber][5] = t6;
				segmentTime[segmentnumber][6] = t7;
				lengthNew[segmentnumber][0] = s1; lengthNew[segmentnumber][1] = s2; lengthNew[segmentnumber][2] = s3;
				lengthNew[segmentnumber][3] = s4; lengthNew[segmentnumber][4] = s5; lengthNew[segmentnumber][5] = s6;
				lengthNew[segmentnumber][6] = s7;
				lengthNewSum[segmentnumber][0] = s1; lengthNewSum[segmentnumber][1] = s1 + s2; lengthNewSum[segmentnumber][2] = s1 + s2 + s3;
				lengthNewSum[segmentnumber][3] = s1 + s2 + s3 + s4; lengthNewSum[segmentnumber][4] = s1 + s2 + s3 + s4 + s5; lengthNewSum[segmentnumber][5] = s1 + s2 + s3 + s4 + s5 + s6;
				lengthNewSum[segmentnumber][6] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
				ssum[segmentnumber] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
			}
			else
			{
				segmentVmax[segmentnumber] = (-amax * t1 + sqrt(pow((amax * t1), 2) - 4 * (-amax * everyLength[segmentnumber]))) / (2 * 1);
				double t1 = amax / jmax;
				double t2 = (segmentVmax[segmentnumber] - vlimit11) / amax;
				double t3 = t2 + t1, t4 = t3 + 0;
				double t5 = t4 + t1, t6 = t5 + ((segmentVmax[segmentnumber] - vlimit12) / amax - t1), t7 = t6 + t1;
				double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
				double s2 = vlimit11 * (t2-t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
				double s3 = segmentVmax[segmentnumber] * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - segmentVmax[segmentnumber] * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
				double s4 = segmentVmax[segmentnumber] * (t4 - t3);
				double s5 = segmentVmax[segmentnumber] * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - segmentVmax[segmentnumber] * t4 + jmax * pow(t4, 3) / 6;
				double s6 = vlimit12 * (t6-t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
				double s7 = vlimit12 * (t7-t6) + 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
				segmentTime[segmentnumber][0] = t1; segmentTime[segmentnumber][1] = t2; segmentTime[segmentnumber][2] = t3;
				segmentTime[segmentnumber][3] = t4; segmentTime[segmentnumber][4] = t5; segmentTime[segmentnumber][5] = t6;
				segmentTime[segmentnumber][6] = t7;
				lengthNew[segmentnumber][0] = s1; lengthNew[segmentnumber][1] = s2; lengthNew[segmentnumber][2] = s3;
				lengthNew[segmentnumber][3] = s4; lengthNew[segmentnumber][4] = s5; lengthNew[segmentnumber][5] = s6;
				lengthNew[segmentnumber][6] = s7;
				lengthNewSum[segmentnumber][0] = s1; lengthNewSum[segmentnumber][1] = s1 + s2; lengthNewSum[segmentnumber][2] = s1 + s2 + s3;
				lengthNewSum[segmentnumber][3] = s1 + s2 + s3 + s4; lengthNewSum[segmentnumber][4] = s1 + s2 + s3 + s4 + s5; lengthNewSum[segmentnumber][5] = s1 + s2 + s3 + s4 + s5 + s6;
				lengthNewSum[segmentnumber][6] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
				ssum[segmentnumber] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
			}
		}
		//第一段
		if (everyLength[0] >= sa1)
		{
			double vlimit11 = 0;
			double deltatime34 = (everyLength[0] - sa1) / vmax;
			double t1 = amax / jmax;
			double t2 = (vmax - vlimit11) / amax;
			double t3 = t2 + t1, t4 = t3 + deltatime34;
			double t5 = t4 + t1, t6 = t5 + ((vmax - vlimit12) / amax - t1), t7 = t6 + t1;
			double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
			double s2 = vlimit11 * (t2 - t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
			double s3 = vmax * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - vmax * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
			double s4 = vmax * (t4 - t3);
			double s5 = vmax * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - vmax * t4 + jmax * pow(t4, 3) / 6;
			double s6 = vlimit12 * (t6 - t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
			double s7 = vlimit12 * (t7 - t6) + 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
			segmentTime[0][0] = t1; segmentTime[0][1] = t2; segmentTime[0][2] = t3;
			segmentTime[0][3] = t4; segmentTime[0][4] = t5; segmentTime[0][5] = t6;
			segmentTime[0][6] = t7;
			lengthNew[0][0] = s1; lengthNew[0][1] = s2; lengthNew[0][2] = s3;
			lengthNew[0][3] = s4; lengthNew[0][4] = s5; lengthNew[0][5] = s6;
			lengthNew[0][6] = s7;
			lengthNewSum[0][0] = s1; lengthNewSum[0][1] = s1 + s2; lengthNewSum[0][2] = s1 + s2 + s3;
			lengthNewSum[0][3] = s1 + s2 + s3 + s4; lengthNewSum[0][4] = s1 + s2 + s3 + s4 + s5; lengthNewSum[0][5] = s1 + s2 + s3 + s4 + s5 + s6;
			lengthNewSum[0][6] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
			ssum[0] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
		}
		else
		{
			double vlimit11 = 0;
			segmentVmax[0] = (-amax * t1 + sqrt(pow((amax * t1), 2) - 4 * (-amax * everyLength[0]))) / (2 * 1);
			double t1 = amax / jmax;
			double t2 = (segmentVmax[0] - vlimit11) / amax;
			double t3 = t2 + t1, t4 = t3 + 0;
			double t5 = t4 + t1, t6 = t5 + ((segmentVmax[0] - vlimit12) / amax - t1), t7 = t6 + t1;
			double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
			double s2 = vlimit11 * (t2 - t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
			double s3 = segmentVmax[0] * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - segmentVmax[0] * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
			double s4 = segmentVmax[0] * (t4 - t3);
			double s5 = segmentVmax[0] * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - segmentVmax[0] * t4 + jmax * pow(t4, 3) / 6;
			double s6 = vlimit12 * (t6 - t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
			double s7 = vlimit12 * (t7 - t6) + 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
			segmentTime[0][0] = t1; segmentTime[0][1] = t2; segmentTime[0][2] = t3;
			segmentTime[0][3] = t4; segmentTime[0][4] = t5; segmentTime[0][5] = t6;
			segmentTime[0][6] = t7;
			lengthNew[0][0] = s1; lengthNew[0][1] = s2; lengthNew[0][2] = s3;
			lengthNew[0][3] = s4; lengthNew[0][4] = s5; lengthNew[0][5] = s6;
			lengthNew[0][6] = s7;
			lengthNewSum[0][0] = s1; lengthNewSum[0][1] = s1 + s2; lengthNewSum[0][2] = s1 + s2 + s3;
			lengthNewSum[0][3] = s1 + s2 + s3 + s4; lengthNewSum[0][4] = s1 + s2 + s3 + s4 + s5; lengthNewSum[0][5] = s1 + s2 + s3 + s4 + s5 + s6;
			lengthNewSum[0][6] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
			ssum[0] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
		}
		//最后一段
		
		if (everyLength[maxpointnumber] >= sa1)
		{
			double vlimit12 = 0;
			double deltatime34 = (everyLength[maxpointnumber] - sa1) / vmax;
			double t1 = amax / jmax;
			double t2 = (vmax - vlimit11) / amax;
			double t3 = t2 + t1, t4 = t3 + deltatime34;
			double t5 = t4 + t1, t6 = t5 + ((vmax - vlimit12) / amax - t1), t7 = t6 + t1;
			double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
			double s2 = vlimit11 * (t2 - t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
			double s3 = vmax * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - vmax * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
			double s4 = vmax * (t4 - t3);
			double s5 = vmax * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - vmax * t4 + jmax * pow(t4, 3) / 6;
			double s6 = vlimit12 * (t6 - t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
			double s7 = vlimit12 * (t7 - t6) + 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
			segmentTime[maxpointnumber][0] = t1; segmentTime[maxpointnumber][1] = t2; segmentTime[maxpointnumber][2] = t3;
			segmentTime[maxpointnumber][3] = t4; segmentTime[maxpointnumber][4] = t5; segmentTime[maxpointnumber][5] = t6;
			segmentTime[maxpointnumber][6] = t7;
			lengthNew[maxpointnumber][0] = s1; lengthNew[maxpointnumber][1] = s2; lengthNew[maxpointnumber][2] = s3;
			lengthNew[maxpointnumber][3] = s4; lengthNew[maxpointnumber][4] = s5; lengthNew[maxpointnumber][5] = s6;
			lengthNew[maxpointnumber][6] = s7;
			lengthNewSum[maxpointnumber][0] = s1; lengthNewSum[maxpointnumber][1] = s1 + s2; lengthNewSum[maxpointnumber][2] = s1 + s2 + s3;
			lengthNewSum[maxpointnumber][3] = s1 + s2 + s3 + s4; lengthNewSum[maxpointnumber][4] = s1 + s2 + s3 + s4 + s5; lengthNewSum[maxpointnumber][5] = s1 + s2 + s3 + s4 + s5 + s6;
			lengthNewSum[maxpointnumber][6] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
			ssum[maxpointnumber] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
		}
		else
		{
			double vlimit12 = 0;
			segmentVmax[maxpointnumber] = (-amax * t1 + sqrt(pow((amax * t1), 2) - 4 * (-amax * everyLength[maxpointnumber]))) / (2 * 1);
			double t1 = amax / jmax;
			double t2 = (segmentVmax[maxpointnumber] - vlimit11) / amax;
			double t3 = t2 + t1, t4 = t3 + 0;
			double t5 = t4 + t1, t6 = t5 + ((segmentVmax[maxpointnumber] - vlimit12) / amax - t1), t7 = t6 + t1;
			double s1 = vlimit11 * t1 + jmax * pow(t1, 3) / 6; //第一段长度
			double s2 = vlimit11 * (t2 - t1) - 0.5 * amax * t1 * t2 + 0.5 * amax * pow(t2, 2); // 第二段长度
			double s3 = segmentVmax[maxpointnumber] * t3 - 0.5 * jmax * (pow(t3, 3) / 3 - t3 * pow(t3, 2) + pow(t3, 2) * t3) - segmentVmax[maxpointnumber] * t2 + 0.5 * jmax * (pow(t2, 3) / 3 - t3 * pow(t2, 2) + pow(t3, 2) * t2);
			double s4 = segmentVmax[maxpointnumber] * (t4 - t3);
			double s5 = segmentVmax[maxpointnumber] * t5 - 0.5 * jmax * (pow(t5, 3) / 3 - t4 * pow(t5, 2) + pow(t4, 2) * t5) - segmentVmax[maxpointnumber] * t4 + jmax * pow(t4, 3) / 6;
			double s6 = vlimit12 * (t6 - t5) + 0.5 * amax * t1 * t6 - 0.5 * amax * pow(t6, 2) + amax * t6 * t6 - 0.5 * amax * t1 * t5 - amax * (t5 * t6 - 0.5 * pow(t5, 2));
			double s7 = vlimit12 * (t7 - t6) + 0.5 * jmax * (pow(t7, 2) * t7 - t7 * pow(t7, 2) + pow(t7, 3) / 3) - 0.5 * jmax * (pow(t7, 2) * t6 - t7 * pow(t6, 2) + pow(t6, 3) / 3);
			segmentTime[maxpointnumber][0] = t1; segmentTime[maxpointnumber][1] = t2; segmentTime[maxpointnumber][2] = t3;
			segmentTime[maxpointnumber][3] = t4; segmentTime[maxpointnumber][4] = t5; segmentTime[maxpointnumber][5] = t6;
			segmentTime[maxpointnumber][6] = t7;
			lengthNew[maxpointnumber][0] = s1; lengthNew[maxpointnumber][1] = s2; lengthNew[maxpointnumber][2] = s3;
			lengthNew[maxpointnumber][3] = s4; lengthNew[maxpointnumber][4] = s5; lengthNew[maxpointnumber][5] = s6;
			lengthNew[maxpointnumber][6] = s7;
			lengthNewSum[maxpointnumber][0] = s1; lengthNewSum[maxpointnumber][1] = s1 + s2; lengthNewSum[maxpointnumber][2] = s1 + s2 + s3;
			lengthNewSum[maxpointnumber][3] = s1 + s2 + s3 + s4; lengthNewSum[maxpointnumber][4] = s1 + s2 + s3 + s4 + s5; lengthNewSum[maxpointnumber][5] = s1 + s2 + s3 + s4 + s5 + s6;
			lengthNewSum[maxpointnumber][6] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
			ssum[maxpointnumber] = s1 + s2 + s3 + s4 + s5 + s6 + s7;
		}
		
		// cout << ssum[0] << endl;
		// cout << ssum[1] << endl;
		// cout << ssum[2] << endl;
		//	//插补 计算u-L的对应关系
		double row = 20000, colume = segmentationnumber;
		vector<vector<double>> uM(row, vector<double>(colume, 0));
		vector<vector<double>> lM(row, vector<double>(colume, 0));
		double numberfirst = 0, numberlast = 0;
		vector<double>uNumber(segmentationnumber - 2, 0);
		
		for (double suml = 0; suml <= segmentLengthSum[segmentationnumber]; suml = suml + 0.0001)
		{
			if (suml <= segmentLengthSum[1])
			{
				uM[numberfirst][0] = cendpar * (1 / (segmentationnumber * segmentLengthSum[1]) * suml);
				lM[numberfirst][0] = suml;
				numberfirst++;
			}
			for (int m = 2; m <= segmentationnumber - 1; m = m + 1)
			{
				if (suml >= segmentLengthSum[m - 1] && suml <= segmentLengthSum[m])
				{
					double a = uSum[m - 1];
					double b = 1 / denominator_1[m - 1];
					double c = ((uSum[m] - uSum[m - 1]) - 1 / denominator_1[m - 1] * (segmentLengthSum[m] - segmentLengthSum[m - 1])) / pow((segmentLengthSum[m] - segmentLengthSum[m - 1]), 2);
					double d = (1 / denominator_1[m] + 1 / denominator_1[m - 1]) / pow(segmentLengthSum[m] - segmentLengthSum[m - 1], 2) - 2 * (uSum[m] - uSum[m - 1]) / pow(segmentLengthSum[m] - segmentLengthSum[m - 1], 3);
					uM[uNumber[m - 2]][m - 1] = a + b * (suml - segmentLengthSum[m - 1]) + c * pow(suml - segmentLengthSum[m - 1], 2) + d * pow(suml - segmentLengthSum[m - 1], 2) * (suml - segmentLengthSum[m]);
					lM[uNumber[m - 2]][m - 1] = suml;
					uNumber[m - 2] = uNumber[m - 2] + 1;

				}
			}
			if (suml >= segmentLengthSum[segmentationnumber - 1] && suml <= segmentLengthSum[segmentationnumber])
			{
				uM[numberlast][segmentationnumber - 1] = cendpar * (suml / (segmentationnumber * (segmentLengthSum[segmentationnumber] - segmentLengthSum[segmentationnumber - 1])) + 1 - segmentLengthSum[segmentationnumber] / (segmentationnumber * (segmentLengthSum[segmentationnumber] - segmentLengthSum[segmentationnumber - 1])));
				lM[numberlast][segmentationnumber - 1] = suml;
				numberlast++;
			}
		}
		int umnozero = 0; //定义一个统计非零元素个数的变量，并初始化为零

		for (int i = 0; i <= colume - 1; i++)//列上限为200
			for (int j = 0; j <= row - 1; j++)//行上限为3000
			{
				if (uM[j][i] > 0)
				{
					umnozero++;
				}
			}
		//cout << endl << "um非零元素个数为：" << umnozero << endl; //输出非零元素个数
		
		vector<double>uM1(umnozero, 0);
		vector<double>lM1(umnozero, 0);
		int nozeroNumber = 0;
		for (int i = 0; i <= colume - 1; i++)//列上限为200
			for (int j = 0; j <= row - 1; j++)//行上限为3000
			{
				if (uM[j][i] > 0)
				{
					uM1[nozeroNumber] = uM[j][i];
					lM1[nozeroNumber] = lM[j][i];
					//cout << lM1[nozeroNumber] << endl;
					nozeroNumber = nozeroNumber + 1;
				}
			}
		//cout << uM1[umnozero - 1] << endl;
		//cout << lM1[umnozero - 1] << endl;
		//	//S型曲线规划
		//int segmenttimeRow = size(everyLength), segmenttimeColumn = 7;行和列
		vector<vector<double>> everyTimestep(segmenttimeRow, vector<double>(segmenttimeColumn, 0));//每个时间段计数的步长
		vector< vector < vector<double> > > lengthEveryTimestep(segmenttimeRow, vector< vector<double> >(500, vector<double>(segmenttimeColumn, 0)));//vector创建三维数组，并全部初始化为1
		vector< vector < vector<double> > > velocityEveryTimestep(segmenttimeRow, vector< vector<double> >(500, vector<double>(segmenttimeColumn, 0)));//vector创建三维数组，并全部初始化为1
		vector<double> sumT(segmenttimeRow + 1, 0);//总时间
		vector<double> sumS(segmenttimeRow + 1, 0);//总长度
		for (int i = 0; i <= segmenttimeRow - 1; i++)
		{
			sumT[i + 1] = sumT[i] + segmentTime[i][6];
			sumS[i + 1] = sumS[i] + ssum[i];
		}
		
		for (double t_d = 0.02; t_d <= sumT[segmenttimeRow]; t_d = t_d + 0.02)
		{
			//第一段
			double vlimit01 = 0, vlimit02 = 2;
			if (t_d > sumT[0] && t_d <= segmentTime[0][0] + sumT[0]) // 第一段
			{
				lengthEveryTimestep[0][everyTimestep[0][0]][0] = vlimit01 * (t_d - sumT[0]) + jmax * pow((t_d - sumT[0]), 3) / 6 + sumS[0];
				velocityEveryTimestep[0][everyTimestep[0][0]][0] = vlimit01 + 0.5 * jmax * pow((t_d - sumT[0]), 2);
				everyTimestep[0][0] = everyTimestep[0][0] + 1;
			}

			if (t_d > segmentTime[0][0] + sumT[0] && t_d <= segmentTime[0][1] + sumT[0]) // 第二段
			{
				lengthEveryTimestep[0][everyTimestep[0][1]][1] = vlimit01 * (t_d - sumT[0]- segmentTime[0][0]) - 0.5 * amax * segmentTime[0][0] * (t_d - sumT[0]) + 0.5 * amax * pow((t_d - sumT[0]), 2) + sumS[0] + lengthNewSum[0][0];
				velocityEveryTimestep[0][everyTimestep[0][1]][1] = vlimit01 + 0.5 * amax * segmentTime[0][0] + amax * (t_d - sumT[0] - segmentTime[0][0]);
				everyTimestep[0][1] = everyTimestep[0][1] + 1;
			}
			if (t_d > segmentTime[0][1] + sumT[0] && t_d <= segmentTime[0][2] + sumT[0]) // 第三段
			{
				lengthEveryTimestep[0][everyTimestep[0][2]][2] = segmentVmax[0] * (t_d - sumT[0]) - 0.5 * jmax * (pow((t_d - sumT[0]), 3) / 3 - segmentTime[0][2] * pow((t_d - sumT[0]), 2) + pow(segmentTime[0][2], 2) * (t_d - sumT[0])) - segmentVmax[0] * segmentTime[0][1] + 0.5 * jmax * (pow(segmentTime[0][1], 3) / 3 - segmentTime[0][2] * pow(segmentTime[0][1], 2) + pow(segmentTime[0][2], 2) * segmentTime[0][1]) + sumS[0] + lengthNewSum[0][1];
				velocityEveryTimestep[0][everyTimestep[0][2]][2] = segmentVmax[0] - 0.5 * jmax * pow((t_d - sumT[0]) - segmentTime[0][2], 2);
				everyTimestep[0][2] = everyTimestep[0][2] + 1;
			}
			if (t_d > segmentTime[0][2] + sumT[0] && t_d <= segmentTime[0][3] + sumT[0]) // 第四段
			{
				lengthEveryTimestep[0][everyTimestep[0][3]][3] = segmentVmax[0] * (t_d - sumT[0] - segmentTime[0][2]) + sumS[0] + lengthNewSum[0][2];
				velocityEveryTimestep[0][everyTimestep[0][3]][3] = segmentVmax[0];
				everyTimestep[0][3] = everyTimestep[0][3] + 1;

			}
			if (t_d > segmentTime[0][3] + sumT[0] && t_d <= segmentTime[0][4] + sumT[0]) // 第五段
			{
				lengthEveryTimestep[0][everyTimestep[0][4]][4] = segmentVmax[0] * (t_d - sumT[0]) - 0.5 * jmax * (pow((t_d - sumT[0]), 3) / 3 - segmentTime[0][3] * pow((t_d - sumT[0]), 2) + pow(segmentTime[0][3], 2) * (t_d - sumT[0])) - segmentVmax[0] * segmentTime[0][3] + jmax * pow(segmentTime[0][3], 3) / 6 + sumS[0] + lengthNewSum[0][3];
				velocityEveryTimestep[0][everyTimestep[0][4]][4] = segmentVmax[0] - 0.5 * jmax * pow((t_d - sumT[0]) - segmentTime[0][3], 2);
				everyTimestep[0][4] = everyTimestep[0][4] + 1;
			}
			if (t_d > segmentTime[0][4] + sumT[0] && t_d <= segmentTime[0][5] + sumT[0]) // 第六段
			{
				lengthEveryTimestep[0][everyTimestep[0][5]][5] = vlimit02 * (t_d - sumT[0]- segmentTime[0][4]) + 0.5 * amax * segmentTime[0][0] * (t_d - sumT[0]) - 0.5 * amax * pow((t_d - sumT[0]), 2) + amax * segmentTime[0][5] * (t_d - sumT[0]) - 0.5 * amax * segmentTime[0][0] * segmentTime[0][4] - amax * (segmentTime[0][4] * segmentTime[0][5] - 0.5 * pow(segmentTime[0][4], 2)) + sumS[0] + lengthNewSum[0][4];
				velocityEveryTimestep[0][everyTimestep[0][5]][5] = vlimit02 + 0.5 * amax * (segmentTime[0][4] - segmentTime[0][3]) + amax * (segmentTime[0][5] - (t_d - sumT[0]));
				everyTimestep[0][5] = everyTimestep[0][5] + 1;
			}
			if (t_d > segmentTime[0][5] + sumT[0] && t_d <= segmentTime[0][6] + sumT[0]) // 第七段
			{
				lengthEveryTimestep[0][everyTimestep[0][6]][6] = vlimit02 * (t_d - sumT[0]- segmentTime[0][5]) + 0.5 * jmax * (pow(segmentTime[0][6], 2) * (t_d - sumT[0]) - segmentTime[0][6] * pow((t_d - sumT[0]), 2) + pow(t_d - sumT[0], 3) / 3) - 0.5 * jmax * (pow(segmentTime[0][6], 2) * segmentTime[0][5] - segmentTime[0][6] * pow(segmentTime[0][5], 2) + pow(segmentTime[0][5], 3) / 3) + sumS[0] + lengthNewSum[0][5];
				velocityEveryTimestep[0][everyTimestep[0][6]][6] = vlimit02 + 0.5 * jmax * pow(segmentTime[0][6] - (t_d - sumT[0]), 2);
				everyTimestep[0][6] = everyTimestep[0][6] + 1;
			}
			//中间段
			for (int i =0; i <= segmenttimeRow - 1; i++)
			{

				if (t_d > sumT[i] && t_d <= segmentTime[i][0] + sumT[i]) // 第一段
				{
					lengthEveryTimestep[i][everyTimestep[i][0]][0] = vlimit11 * (t_d - sumT[i]) + jmax * pow((t_d - sumT[i]), 3) / 6 + sumS[i];
					velocityEveryTimestep[i][everyTimestep[i][0]][0] = vlimit11 + 0.5 * jmax * pow((t_d - sumT[i]), 2);
					everyTimestep[i][0] = everyTimestep[i][0] + 1;
				}

				if (t_d > segmentTime[i][0] + sumT[i] && t_d <= segmentTime[i][1] + sumT[i]) // 第二段
				{
					lengthEveryTimestep[i][everyTimestep[i][1]][1] = vlimit11 * (t_d - sumT[i]- segmentTime[i][0]) - 0.5 * amax * segmentTime[i][0] * (t_d - sumT[i]) + 0.5 * amax * pow((t_d - sumT[i]), 2) + sumS[i] + lengthNewSum[i][0];
					velocityEveryTimestep[i][everyTimestep[i][1]][1] = vlimit11 + 0.5 * amax * segmentTime[i][0] + amax * (t_d - sumT[i] - segmentTime[i][0]);
					everyTimestep[i][1] = everyTimestep[i][1] + 1;
				}
				if (t_d > segmentTime[i][1] + sumT[i] && t_d <= segmentTime[i][2] + sumT[i]) // 第三段
				{
					lengthEveryTimestep[i][everyTimestep[i][2]][2] = segmentVmax[i] * (t_d - sumT[i]) - 0.5 * jmax * (pow((t_d - sumT[i]), 3) / 3 - segmentTime[i][2] * pow((t_d - sumT[i]), 2) + pow(segmentTime[i][2], 2) * (t_d - sumT[i])) - segmentVmax[i] * segmentTime[i][1] + 0.5 * jmax * (pow(segmentTime[i][1], 3) / 3 - segmentTime[i][2] * pow(segmentTime[i][1], 2) + pow(segmentTime[i][2], 2) * segmentTime[i][1]) + sumS[i] + lengthNewSum[i][1];
					velocityEveryTimestep[i][everyTimestep[i][2]][2] = segmentVmax[i] - 0.5 * jmax * pow((t_d - sumT[i]) - segmentTime[i][2], 2);
					everyTimestep[i][2] = everyTimestep[i][2] + 1;
				}
				if (t_d > segmentTime[i][2] + sumT[i] && t_d <= segmentTime[i][3] + sumT[i]) // 第四段
				{
					lengthEveryTimestep[i][everyTimestep[i][3]][3] = segmentVmax[i] * (t_d - sumT[i] - segmentTime[i][2]) + sumS[i] + lengthNewSum[i][2];
					velocityEveryTimestep[i][everyTimestep[i][3]][3] = segmentVmax[i];
					everyTimestep[i][3] = everyTimestep[i][3] + 1;
					
				}
				if (t_d > segmentTime[i][3] + sumT[i] && t_d <= segmentTime[i][4] + sumT[i]) // 第五段
				{
					lengthEveryTimestep[i][everyTimestep[i][4]][4] = segmentVmax[i] * (t_d - sumT[i]) - 0.5 * jmax * (pow((t_d - sumT[i]), 3) / 3 - segmentTime[i][3] * pow((t_d - sumT[i]), 2) + pow(segmentTime[i][3], 2) * (t_d - sumT[i])) - segmentVmax[i] * segmentTime[i][3] + jmax * pow(segmentTime[i][3], 3) / 6 + sumS[i] + lengthNewSum[i][3];
					velocityEveryTimestep[i][everyTimestep[i][4]][4] = segmentVmax[i] - 0.5 * jmax * pow((t_d - sumT[i]) - segmentTime[i][3], 2);
					everyTimestep[i][4] = everyTimestep[i][4] + 1;
				}
				if (t_d > segmentTime[i][4] + sumT[i] && t_d <= segmentTime[i][5] + sumT[i]) // 第六段
				{
					lengthEveryTimestep[i][everyTimestep[i][5]][5] = vlimit12 * (t_d - sumT[i]- segmentTime[i][4]) + 0.5 * amax * segmentTime[i][0] * (t_d - sumT[i]) - 0.5 * amax * pow((t_d - sumT[i]), 2) + amax * segmentTime[i][5] * (t_d - sumT[i]) - 0.5 * amax * segmentTime[i][0] * segmentTime[i][4] - amax * (segmentTime[i][4] * segmentTime[i][5] - 0.5 * pow(segmentTime[i][4], 2)) + sumS[i] + lengthNewSum[i][4];
					velocityEveryTimestep[i][everyTimestep[i][5]][5] = vlimit12 + 0.5 * amax * (segmentTime[i][4] - segmentTime[i][3]) + amax * (segmentTime[i][5] - (t_d - sumT[i]));
					everyTimestep[i][5] = everyTimestep[i][5] + 1;
				}
				if (t_d > segmentTime[i][5] + sumT[i] && t_d <= segmentTime[i][6] + sumT[i]) // 第七段
				{
					lengthEveryTimestep[i][everyTimestep[i][6]][6] = vlimit12 * (t_d - sumT[i]- segmentTime[i][5]) + 0.5 * jmax * (pow(segmentTime[i][6], 2) * (t_d - sumT[i]) - segmentTime[i][6] * pow((t_d - sumT[i]), 2) + pow(t_d - sumT[i], 3) / 3) - 0.5 * jmax * (pow(segmentTime[i][6], 2) * segmentTime[i][5] - segmentTime[i][6] * pow(segmentTime[i][5], 2) + pow(segmentTime[i][5], 3) / 3) + sumS[i] + lengthNewSum[i][5];
					velocityEveryTimestep[i][everyTimestep[i][6]][6] = vlimit12 + 0.5 * jmax * pow(segmentTime[i][6] - (t_d - sumT[i]), 2);
					everyTimestep[i][6] = everyTimestep[i][6] + 1;
				}
			}
			
		//最后一段
			double vlimit21 = 2, vlimit22 = 0;
			if (t_d > sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][0] + sumT[segmenttimeRow - 1]) // 第一段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][0]][0] = vlimit21 * (t_d - sumT[segmenttimeRow - 1]) + jmax * pow((t_d - sumT[segmenttimeRow - 1]), 3) / 6 + sumS[segmenttimeRow - 1];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][0]][0] = vlimit21 + 0.5 * jmax * pow((t_d - sumT[segmenttimeRow - 1]), 2);
				everyTimestep[segmenttimeRow - 1][0] = everyTimestep[segmenttimeRow - 1][0] + 1;
			}

			if (t_d > segmentTime[segmenttimeRow - 1][0] + sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][1] + sumT[segmenttimeRow - 1]) // 第二段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][1]][1] = vlimit21 * (t_d - sumT[segmenttimeRow - 1]- segmentTime[segmenttimeRow - 1][0]) - 0.5 * amax * segmentTime[segmenttimeRow - 1][0] * (t_d - sumT[segmenttimeRow - 1]) + 0.5 * amax * pow((t_d - sumT[segmenttimeRow - 1]), 2) + sumS[segmenttimeRow - 1] + lengthNewSum[segmenttimeRow - 1][0];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][1]][1] = vlimit21 + 0.5 * amax * segmentTime[segmenttimeRow - 1][0] + amax * (t_d - sumT[segmenttimeRow - 1] - segmentTime[segmenttimeRow - 1][0]);
				everyTimestep[segmenttimeRow - 1][1] = everyTimestep[segmenttimeRow - 1][1] + 1;
			}
			if (t_d > segmentTime[segmenttimeRow - 1][1] + sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][2] + sumT[segmenttimeRow - 1]) // 第三段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][2]][2] = segmentVmax[segmenttimeRow - 1] * (t_d - sumT[segmenttimeRow - 1]) - 0.5 * jmax * (pow((t_d - sumT[segmenttimeRow - 1]), 3) / 3 - segmentTime[segmenttimeRow - 1][2] * pow((t_d - sumT[segmenttimeRow - 1]), 2) + pow(segmentTime[segmenttimeRow - 1][2], 2) * (t_d - sumT[segmenttimeRow - 1])) - segmentVmax[segmenttimeRow - 1] * segmentTime[segmenttimeRow - 1][1] + 0.5 * jmax * (pow(segmentTime[segmenttimeRow - 1][1], 3) / 3 - segmentTime[segmenttimeRow - 1][2] * pow(segmentTime[segmenttimeRow - 1][1], 2) + pow(segmentTime[segmenttimeRow - 1][2], 2) * segmentTime[segmenttimeRow - 1][1]) + sumS[segmenttimeRow - 1] + lengthNewSum[segmenttimeRow - 1][1];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][2]][2] = segmentVmax[segmenttimeRow - 1] - 0.5 * jmax * pow((t_d - sumT[segmenttimeRow - 1]) - segmentTime[segmenttimeRow - 1][2], 2);
				everyTimestep[segmenttimeRow - 1][2] = everyTimestep[segmenttimeRow - 1][2] + 1;
			}
			if (t_d > segmentTime[segmenttimeRow - 1][2] + sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][3] + sumT[segmenttimeRow - 1]) // 第四段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][3]][3] = segmentVmax[segmenttimeRow - 1] * (t_d - sumT[segmenttimeRow - 1] - segmentTime[segmenttimeRow - 1][2]) + sumS[segmenttimeRow - 1] + lengthNewSum[segmenttimeRow - 1][2];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][3]][3] = segmentVmax[segmenttimeRow - 1];
				everyTimestep[segmenttimeRow - 1][3] = everyTimestep[segmenttimeRow - 1][3] + 1;

			}
			if (t_d > segmentTime[segmenttimeRow - 1][3] + sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][4] + sumT[segmenttimeRow - 1]) // 第五段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][4]][4] = segmentVmax[segmenttimeRow - 1] * (t_d - sumT[segmenttimeRow - 1]) - 0.5 * jmax * (pow((t_d - sumT[segmenttimeRow - 1]), 3) / 3 - segmentTime[segmenttimeRow - 1][3] * pow((t_d - sumT[segmenttimeRow - 1]), 2) + pow(segmentTime[segmenttimeRow - 1][3], 2) * (t_d - sumT[segmenttimeRow - 1])) - segmentVmax[segmenttimeRow - 1] * segmentTime[segmenttimeRow - 1][3] + jmax * pow(segmentTime[segmenttimeRow - 1][3], 3) / 6 + sumS[segmenttimeRow - 1] + lengthNewSum[segmenttimeRow - 1][3];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][4]][4] = segmentVmax[segmenttimeRow - 1] - 0.5 * jmax * pow((t_d - sumT[segmenttimeRow - 1]) - segmentTime[segmenttimeRow - 1][3], 2);
				everyTimestep[segmenttimeRow - 1][4] = everyTimestep[segmenttimeRow - 1][4] + 1;
			}
			if (t_d > segmentTime[segmenttimeRow - 1][4] + sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][5] + sumT[segmenttimeRow - 1]) // 第六段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][5]][5] = vlimit22 * (t_d - sumT[segmenttimeRow - 1]- segmentTime[segmenttimeRow - 1][4]) + 0.5 * amax * segmentTime[segmenttimeRow - 1][0] * (t_d - sumT[segmenttimeRow - 1]) - 0.5 * amax * pow((t_d - sumT[segmenttimeRow - 1]), 2) + amax * segmentTime[segmenttimeRow - 1][5] * (t_d - sumT[segmenttimeRow - 1]) - 0.5 * amax * segmentTime[segmenttimeRow - 1][0] * segmentTime[segmenttimeRow - 1][4] - amax * (segmentTime[segmenttimeRow - 1][4] * segmentTime[segmenttimeRow - 1][5] - 0.5 * pow(segmentTime[segmenttimeRow - 1][4], 2)) + sumS[segmenttimeRow - 1] + lengthNewSum[segmenttimeRow - 1][4];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][5]][5] = vlimit22 + 0.5 * amax * (segmentTime[segmenttimeRow - 1][4] - segmentTime[segmenttimeRow - 1][3]) + amax * (segmentTime[segmenttimeRow - 1][5] - (t_d - sumT[segmenttimeRow - 1]));
				everyTimestep[segmenttimeRow - 1][5] = everyTimestep[segmenttimeRow - 1][5] + 1;
			}
			if (t_d > segmentTime[segmenttimeRow - 1][5] + sumT[segmenttimeRow - 1] && t_d <= segmentTime[segmenttimeRow - 1][6] + sumT[segmenttimeRow - 1]) // 第七段
			{
				lengthEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][6]][6] = vlimit22 * (t_d - sumT[segmenttimeRow - 1]- segmentTime[segmenttimeRow - 1][5]) + 0.5 * jmax * (pow(segmentTime[segmenttimeRow - 1][6], 2) * (t_d - sumT[segmenttimeRow - 1]) - segmentTime[segmenttimeRow - 1][6] * pow((t_d - sumT[segmenttimeRow - 1]), 2) + pow(t_d - sumT[segmenttimeRow - 1], 3) / 3) - 0.5 * jmax * (pow(segmentTime[segmenttimeRow - 1][6], 2) * segmentTime[segmenttimeRow - 1][5] - segmentTime[segmenttimeRow - 1][6] * pow(segmentTime[segmenttimeRow - 1][5], 2) + pow(segmentTime[segmenttimeRow - 1][5], 3) / 3) + sumS[segmenttimeRow - 1] + lengthNewSum[segmenttimeRow - 1][5];
				velocityEveryTimestep[segmenttimeRow - 1][everyTimestep[segmenttimeRow - 1][6]][6] = vlimit22 + 0.5 * jmax * pow(segmentTime[segmenttimeRow - 1][6] - (t_d - sumT[segmenttimeRow - 1]), 2);
				everyTimestep[segmenttimeRow - 1][6] = everyTimestep[segmenttimeRow - 1][6] + 1;
			}
		}
		// cout << lengthEveryTimestep[0][0][6] << endl;
		//cout << sumS[1] + lengthNewSum[1][2] << endl;

		//寻找t和L的关系
		int noZero = 0; //定义一个统计非零元素个数的变量，并初始化为零
		vector<double>LM2;
		for (int i = 0; i <= segmenttimeRow - 1; i++)//第n段函数
			for (int j = 0; j <= segmenttimeColumn - 1; j++)//列
				for (int k = 0; k <= 300 - 1; k++)//行
				{
					{
						if (lengthEveryTimestep[i][k][j] > 0)
						{
							LM2.push_back(lengthEveryTimestep[i][k][j]);
							//cout << LM2[noZero] << endl;
							noZero++;
						}
					}
				}
		//cout << endl << "lengthEveryTimestep非零个数为" << noZero << endl; //输出非零元素个数
		//cout << LM2[noZero - 1] << endl;

		vector<double>UM3(noZero, 0);
		int check = 0;
		for (int checkUm = 1; checkUm <= umnozero - 1; checkUm++)//行上限为3000
		{

			for (int checkUm2 = 1; checkUm2 <= noZero - 1; checkUm2++)
			{
				if (LM2[checkUm2 - 1] > lM1[checkUm - 1] && LM2[checkUm2 - 1] <= lM1[checkUm]) // 第七段

				{
					UM3[check] = uM1[checkUm - 1];
					//cout << UM3[check] << endl;
					check++;
				}
			}
		}
		//cout << "插值点数据的个数" << check << endl;
		
		vector<vector<double>>interpolationPosition(check, vector<double>(3, 0));
		for (int checkUm = 0; checkUm <=check-1; checkUm++)
		{
			double parvalue = UM3[checkUm];
			s1225(result_curve, der, parvalue, &UKnotspan, derive, curvature, &radius_of_curvature, &jstat);

				interpolationPosition[checkUm][0] = derive[0];
				interpolationPosition[checkUm][1] = derive[1];
				interpolationPosition[checkUm][2] = derive[2];
				catersianPosition[0] = derive[0];
				catersianPosition[1] = derive[1];
				catersianPosition[2] = derive[2];
				catesianSpacePosition.push_back(catersianPosition);
				//cout << catesianSpacePosition[checkUm][0] << "  "; cout << catesianSpacePosition[checkUm][1] << "  "; cout << catesianSpacePosition[checkUm][2] << endl;
		}
		// cout << "位置的数量" << catesianSpacePosition.size() << endl;
		//自由曲线的姿态
		Matrix3d  R1, R2,R3,R4, R, RTrans2, RTrans3;
		R1 << cos(posture[0][2]) * cos(posture[0][1]), cos(posture[0][2])* sin(posture[0][1])* sin(posture[0][0]) - sin(posture[0][2]) * cos(posture[0][0]), cos(posture[0][2])* sin(posture[0][1])* cos(posture[0][0]) + sin(posture[0][2]) * sin(posture[0][0]),
			sin(posture[0][2])* cos(posture[0][1]), sin(posture[0][2])* sin(posture[0][1])* sin(posture[0][0]) + cos(posture[0][2]) * cos(posture[0][0]), sin(posture[0][2])* sin(posture[0][1])* cos(posture[0][0]) - cos(posture[0][2]) * sin(posture[0][0]),
			-sin(posture[0][1]), cos(posture[0][1])* sin(posture[0][0]), cos(posture[0][1])* cos(posture[0][0]);
		R2 << cos(posture[1][2]) * cos(posture[1][1]), cos(posture[1][2])* sin(posture[1][1])* sin(posture[1][0]) - sin(posture[1][2]) * cos(posture[1][0]), cos(posture[1][2])* sin(posture[1][1])* cos(posture[1][0]) + sin(posture[1][2]) * sin(posture[1][0]),
			sin(posture[1][2])* cos(posture[1][1]), sin(posture[1][2])* sin(posture[1][1])* sin(posture[1][0]) + cos(posture[1][2]) * cos(posture[1][0]), sin(posture[1][2])* sin(posture[1][1])* cos(posture[1][0]) - cos(posture[1][2]) * sin(posture[1][0]),
			-sin(posture[1][1]), cos(posture[1][1])* sin(posture[1][0]), cos(posture[1][1])* cos(posture[1][0]);
		R3 << cos(posture[2][2]) * cos(posture[2][1]), cos(posture[2][2])* sin(posture[2][1])* sin(posture[2][0]) - sin(posture[2][2]) * cos(posture[2][0]), cos(posture[2][2])* sin(posture[2][1])* cos(posture[2][0]) + sin(posture[2][2]) * sin(posture[2][0]),
			sin(posture[2][2])* cos(posture[2][1]), sin(posture[2][2])* sin(posture[2][1])* sin(posture[2][0]) + cos(posture[2][2]) * cos(posture[2][0]), sin(posture[2][2])* sin(posture[2][1])* cos(posture[2][0]) - cos(posture[2][2]) * sin(posture[2][0]),
			-sin(posture[2][1]), cos(posture[2][1])* sin(posture[2][0]), cos(posture[2][1])* cos(posture[2][0]);
		R4 << cos(posture[posture.size()-1][2]) * cos(posture[posture.size()-1][1]), cos(posture[posture.size()-1][2])* sin(posture[posture.size()-1][1])* sin(posture[posture.size()-1][0]) - sin(posture[posture.size()-1][2]) * cos(posture[posture.size()-1][0]), cos(posture[posture.size()-1][2])* sin(posture[posture.size()-1][1])* cos(posture[posture.size()-1][0]) + sin(posture[posture.size()-1][2]) * sin(posture[posture.size()-1][0]),
			sin(posture[posture.size()-1][2])* cos(posture[posture.size()-1][1]), sin(posture[posture.size()-1][2])* sin(posture[posture.size()-1][1])* sin(posture[posture.size()-1][0]) + cos(posture[posture.size()-1][2]) * cos(posture[posture.size()-1][0]), sin(posture[posture.size()-1][2])* sin(posture[posture.size()-1][1])* cos(posture[posture.size()-1][0]) - cos(posture[posture.size()-1][2]) * sin(posture[posture.size()-1][0]),
			-sin(posture[posture.size()-1][1]), cos(posture[posture.size()-1][1])* sin(posture[posture.size()-1][0]), cos(posture[posture.size()-1][1])* cos(posture[posture.size()-1][0]);
		/*cout << R1 << endl; cout << endl;
		cout << R2 << endl; cout << endl;
		cout << R3 << endl; cout << endl;
		cout << R4 << endl; cout << endl;*/
		//多个旋转轴
		//R = R2 * R1.inverse();//转换矩阵
		//RTrans2= R3 * R2.inverse();
		//RTrans3 = R4 * R3.inverse();
		//double quaternion1_0 = 0.5 * sqrt((1 + R(0, 0) + R(1, 1) + R(2, 2)));
		//double quaternion1_1 = (R(2, 1) - R(1, 2)) / (4 * quaternion1_0);
		//double quaternion1_2 = (R(0, 2) - R(2, 0)) / (4 * quaternion1_0);
		//double quaternion1_3 = (R(1, 0) - R(0, 1)) / (4 * quaternion1_0);//四元数

		//double quaternion2_0 = 0.5 * sqrt((1 + RTrans2(0, 0) + RTrans2(1, 1) + RTrans2(2, 2)));
		//double quaternion2_1 = (RTrans2(2, 1) - RTrans2(1, 2)) / (4 * quaternion2_0);
		//double quaternion2_2 = (RTrans2(0, 2) - RTrans2(2, 0)) / (4 * quaternion2_0);
		//double quaternion2_3 = (RTrans2(1, 0) - RTrans2(0, 1)) / (4 * quaternion2_0);//四元数

		//double quaternion3_0 = 0.5 * sqrt((1 + RTrans3(0, 0) + RTrans3(1, 1) + RTrans3(2, 2)));
		//double quaternion3_1 = (RTrans3(2, 1) - RTrans3(1, 2)) / (4 * quaternion3_0);
		//double quaternion3_2 = (RTrans3(0, 2) - RTrans3(2, 0)) / (4 * quaternion3_0);
		//double quaternion3_3 = (RTrans3(1, 0) - RTrans3(0, 1)) / (4 * quaternion3_0);//四元数

		//cout << quaternion1_0 << "    " << quaternion1_1 << "    " << quaternion1_2 << "    " << quaternion1_3 << endl;
		//cout << quaternion2_0 << "    " << quaternion2_1 << "    " << quaternion2_2 << "    " << quaternion2_3 << endl;
		//cout << quaternion3_0 << "    " << quaternion3_1 << "    " << quaternion3_2 << "    " << quaternion3_3 << endl;
		
		//一个旋转轴 
		R= R4 * R1.inverse();
		double quaternion1_0 = 0.5 * sqrt((1 + R(0, 0) + R(1, 1) + R(2, 2)));
		double quaternion1_1 = (R(2, 1) - R(1, 2)) / (4 * quaternion1_0);
		double quaternion1_2 = (R(0, 2) - R(2, 0)) / (4 * quaternion1_0);
		double quaternion1_3 = (R(1, 0) - R(0, 1)) / (4 * quaternion1_0);//四元数

		double rotaryShaftX = quaternion1_1 / sqrt(quaternion1_1 * quaternion1_1 + quaternion1_2 * quaternion1_2 + quaternion1_3 * quaternion1_3);
		double rotaryShaftY = quaternion1_2 / sqrt(quaternion1_1 * quaternion1_1 + quaternion1_2 * quaternion1_2 + quaternion1_3 * quaternion1_3);
		double rotaryShaftZ = quaternion1_3 / sqrt(quaternion1_1 * quaternion1_1 + quaternion1_2 * quaternion1_2 + quaternion1_3 * quaternion1_3);//四元数对应的旋转轴
		double rotaryAngle = acos(quaternion1_0);
		//cout << rotaryAngle << endl;
		if (quaternion1_0 == 1)
		{
			for (int a1 = 0; a1 <= check-1; a1 = a1 + 1)
			{
				catersianPosture[0] = posture[0][0];
				catersianPosture[1] = posture[0][1];
				catersianPosture[2] = posture[0][2];
				catesianSpacePosture.push_back(catersianPosture);
			}
		}
		
		else {
			////四元数存在多个旋转轴时
			//double q_pointsPosture[] = { 1, 0, 0,
			//	   1,0,0,
			//	   1,0,0,
			//	   1,0,0,
			//		 };
			//const int num_points = sizeof(q_pointsPosture) / sizeof(q_pointsPosture[0]) / 3;//                                                                                  需要根据数据个数调整
			//cout << num_points << endl;
			//int typePosture[num_points] = { 0 };
			//for (int i = 0; i <= num_points - 1; i++)
			//{
			//	typePosture[i] = 1;
			//}//需要根据数据个数调整
			///*double n_control = size(u) - p - 2;*/
			//double cendparPosture;
			//SISLCurve* result_curvePosture = 0;
			//double* gparPosture = 0;
			//int jnbparPosture;
			//s1356(q_pointsPosture,        // pointer to where the point coordinates are stored
			//	num_points,    // number of points to be interpolated
			//	3,             // the dimension
			//	typePosture,   // what type of information is stored at a particular point
			//	0,             // no additional condition at start point
			//	0,             // no additional condition at end point
			//	1,             // open curve
			//	4,             // order of the spline curve to be produced
			//	cstartpar,     // parameter value to be used at start of curve
			//	&cendparPosture,      // parameter value at the end of the curve (to be determined)
			//	&result_curvePosture, // the resulting spline curve (to be determined)
			//	&gparPosture,         // pointer to the parameter values of the points in the curve
			//	// (to be determined)
			//	&jnbparPosture,       // number of unique parameter values (to be determined)
			//	&jstat);       // status message
			//if (jstat < 0) {
			//	throw runtime_error("Error occured inside call to SISL routine.");
			//}
			//else if (jstat > 0) {
			//	cerr << "WARNING: warning occured inside call to SISL routine. \n" << endl;
			//}

			////cout << "Total parameter interval of curve: [" << cstartpar << ", "
			//	//<< cendparPosture << "]\n\n";
			////cout << "Point parameter values was decided to be: \n";
			//for (int i = 0; i < num_points; ++i)
			//{
			//	//cout << gparPosture[i] << ' ';
			//}
			////cout << endl;
			////cout << jnbparPosture << endl;
			//double u_startPosture = 0, u_endPosture = gparPosture[num_points - 1];
			////double segmentationnumber = 200;//分割步数
			////double h = (u_endPosture - u_startPosture) / segmentationnumber;

			//double derivePosture[9] = { 0 };/* Curve dimension times (der + 1) */
			//int mPosture = 1000, nPosture = 9;
			//vector < vector<double> > cruve_derPosture(mPosture, vector<double>(n, 1));//vector创建二维数组，曲线上的点 一阶导 二阶导
			///*double s_sum = 0, sum_1 = 0, sum_2 = 0;*/
			////int der = 2;
			//int UKnotspanPosture = { 0 };
			//double curvaturePosture[3] = { 0 }; /* Curve dimension */
			//double radius_of_curvaturePosture = 0;
			//vector<vector<double>> cruvePoint(check, vector<double>(3, 0));//插值点在单位球面上的投影
			//for (int a1 = 0; a1 <= check - 1; a1++)//姿态与位置对应的u要一致
			//{
			//	double parvalue = UM3[check - 1] / cendpar * cendparPosture;
			//	s1225(result_curvePosture, der, parvalue, &UKnotspanPosture, derivePosture, curvaturePosture, &radius_of_curvaturePosture, &jstat);

			//	cruvePoint[a1][0] = derivePosture[0] / sqrt(pow(derivePosture[0], 2) + pow(derivePosture[1], 2) + pow(derivePosture[2], 2));
			//	cruvePoint[a1][1] = derivePosture[1] / sqrt(pow(derivePosture[0], 2) + pow(derivePosture[1], 2) + pow(derivePosture[2], 2));
			//	cruvePoint[a1][2] = derivePosture[2] / sqrt(pow(derivePosture[0], 2) + pow(derivePosture[1], 2) + pow(derivePosture[2], 2));

			//}
			//vector< vector < vector<double> > > Req(3, vector< vector<double> >(3, vector<double>(check, 0)));//vector创建三维数组，并全部初始化为0
			//vector<vector<double>> quaternions(check, vector<double>(4, 0));//在插值过程中的四元数
			//vector<vector<double>> RPY2end(1000, vector<double>(3, 0));
			//for (int i = 0; i <= check - 1; i++)
			//{
			//	quaternions[i][0] = cruvePoint[0][0] * cruvePoint[i][0] + cruvePoint[0][1] * cruvePoint[i][1] + cruvePoint[0][2] * cruvePoint[i][2];
			//	quaternions[i][1] = cruvePoint[0][1] * cruvePoint[i][2] - cruvePoint[0][2] * cruvePoint[i][1];
			//	quaternions[i][2] = cruvePoint[0][2] * cruvePoint[i][0] - cruvePoint[0][0] * cruvePoint[i][2];
			//	quaternions[i][3] = cruvePoint[0][0] * cruvePoint[i][1] - cruvePoint[0][1] * cruvePoint[i][0];
			//	//cout << quaternions[i][0] << " "; cout << quaternions[i][1] << " "; cout << quaternions[i][2] << " "; cout << quaternions[i][3] << endl;
			//	Req[0][0][i] = pow(quaternions[i][0], 2) + pow(quaternions[i][1], 2) - pow(quaternions[i][2], 2) - pow(quaternions[i][3], 2);
			//	Req[0][1][i] = 2 * (quaternions[i][1] * quaternions[i][2] - quaternions[i][0] * quaternions[i][3]);
			//	Req[0][2][i] = 2 * (quaternions[i][0] * quaternions[i][2] + quaternions[i][1] * quaternions[i][3]);
			//	Req[1][0][i] = 2 * (quaternions[i][0] * quaternions[i][3] + quaternions[i][1] * quaternions[i][2]);
			//	Req[1][1][i] = pow(quaternions[i][0], 2) - pow(quaternions[i][1], 2) + pow(quaternions[i][2], 2) - pow(quaternions[i][3], 2);
			//	Req[1][2][i] = 2 * (quaternions[i][2] * quaternions[i][3] - quaternions[i][0] * quaternions[i][1]);
			//	Req[2][0][i] = 2 * (quaternions[i][1] * quaternions[i][3] - quaternions[i][0] * quaternions[i][2]);
			//	Req[2][1][i] = 2 * (quaternions[i][0] * quaternions[i][1] + quaternions[i][2] * quaternions[i][3]);
			//	Req[2][2][i] = pow(quaternions[i][0], 2) - pow(quaternions[i][1], 2) - pow(quaternions[i][2], 2) + pow(quaternions[i][3], 2);
			//	Matrix3d  roraionMatrix, R2ed;
			//	roraionMatrix << Req[0][0][i], Req[0][1][i], Req[0][2][i],
			//		Req[1][0][i], Req[1][1][i], Req[1][2][i],
			//		Req[2][0][i], Req[2][1][i], Req[2][2][i];
			//	R2ed = roraionMatrix * R1;
			//	if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > -PI / 2 && atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < PI / 2)
			//	{
			//		RPY2end[i][0] = atan2(R2ed(2, 1), R2ed(2, 2)); RPY2end[i][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[i][2] = atan2(R2ed(1, 0), R2ed(0, 0));
			//	}
			//	if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < -PI / 2 || atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > PI / 2)
			//	{
			//		RPY2end[i][0] = atan2(-R2ed(2, 1), -R2ed(2, 2)); RPY2end[i][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[i][2] = atan2(-R2ed(1, 0), -R2ed(0, 0));
			//	}
			//	//cout << RPY2end[i][0] << " "; cout << RPY2end[i][1] << " "; cout << RPY2end[i][2] << endl;
			//	catersianPosture[0] = RPY2end[i][0];
			//	catersianPosture[1] = RPY2end[i][1];
			//	catersianPosture[2] = RPY2end[i][2];
			//	catesianSpacePosture.push_back(catersianPosture);
			//}


         //只有一个旋转轴时
			vector<double> angle{ 0,rotaryAngle }, timePosture1{ 0,sumT[segmenttimeRow] }, velocityPosture{ 0,0 }, acclationPosture{ 0,0 };//定义位移及速度矢量
			int j = 0;
			double timePosture = timePosture1[j + 1] - timePosture1[j];//每段规划的时间，各关节相等
			double a01Posture = angle[j];
			double a11Posture = velocityPosture[j];
			double a21Posture = acclationPosture[j] / 2;
			double a31Posture = (20 * angle[j + 1] - 20 * angle[j] - (8 * velocityPosture[j + 1] + 12 * velocityPosture[j]) * timePosture - (3 * acclationPosture[j] - acclationPosture[j + 1]) * pow(timePosture, 2)) / (2 * pow(timePosture, 3));
			double a41Posture = (30 * angle[j] - 30 * angle[j + 1] + (14 * velocityPosture[j + 1] + 16 * velocityPosture[j]) * timePosture + (3 * acclationPosture[j] - 2 * acclationPosture[j + 1]) * pow(timePosture, 2)) / (2 * pow(timePosture, 4));
			double a51Posture = (12 * angle[j + 1] - 12 * angle[j] - (6 * velocityPosture[j + 1] + 6 * velocityPosture[j]) * timePosture - (acclationPosture[j] - acclationPosture[j + 1]) * pow(timePosture, 2)) / (2 * pow(timePosture, 5));
			//int t1_number = (timePosture1[j + 1] - timePosture1[j]) / interplorationTime;
			vector<double> t_array1(5000,0),
				q_array1(5000, 0),
				v_array1(5000, 0),
				a_array1(5000, 0);
			int H = 3000, L = 4;
			int m = 3, n = 3, p = 3000;
			vector<vector<double>> q_1new(H, vector<double>(L, 1));	//vector创建二维数组，并全部初始化为1
			vector< vector < vector<double> > > Req(m, vector< vector<double> >(n, vector<double>(p, 0)));//vector创建三维数组，并全部初始化为0
			vector<vector<double>> RPY2end(3000, vector<double>(3, 0));
			for (int a1 = 1; a1 <= check; a1 = a1 + 1)
			{
				t_array1[a1] = t_array1[a1 - 1] + sumT[segmenttimeRow] / check;
				q_array1[a1] = a01Posture + a11Posture * (t_array1[a1] - timePosture1[j]) + a21Posture * pow((t_array1[a1] - timePosture1[j]), 2) + a31Posture * pow((t_array1[a1] - timePosture1[j]), 3) + a41Posture * pow((t_array1[a1] - timePosture1[j]), 4) + a51Posture * pow((t_array1[a1] - timePosture1[j]), 5);
				v_array1[a1]=(a11Posture + 2 * a21Posture * (t_array1[a1] - timePosture1[j]) + 3 * a31Posture * pow((t_array1[a1] - timePosture1[j]), 2) + 4 * a41Posture * pow((t_array1[a1] - timePosture1[j]), 3) + 5 * a51Posture * pow((t_array1[a1] - timePosture1[j]), 4));
				a_array1[a1] = (2 * a21Posture + 6 * a31Posture * (t_array1[a1] - timePosture1[j]) + 12 * a41Posture * pow((t_array1[a1] - timePosture1[j]), 2) + 20 * a51Posture * pow((t_array1[a1] - timePosture1[j]), 3));
				q_1new[a1][0] = cos(q_array1[a1]);
				q_1new[a1][1] = rotaryShaftX * sin(q_array1[a1]);
				q_1new[a1][2] = rotaryShaftY * sin(q_array1[a1]);
				q_1new[a1][3] = rotaryShaftZ * sin(q_array1[a1]);
				Req[0][0][a1] = pow(q_1new[a1][0], 2) + pow(q_1new[a1][1], 2) - pow(q_1new[a1][2], 2) - pow(q_1new[a1][3], 2);
				Req[0][1][a1] = 2 * (q_1new[a1][1] * q_1new[a1][2] - q_1new[a1][0] * q_1new[a1][3]);
				Req[0][2][a1] = 2 * (q_1new[a1][0] * q_1new[a1][2] + q_1new[a1][1] * q_1new[a1][3]);
				Req[1][0][a1] = 2 * (q_1new[a1][0] * q_1new[a1][3] + q_1new[a1][1] * q_1new[a1][2]);
				Req[1][1][a1] = pow(q_1new[a1][0], 2) - pow(q_1new[a1][1], 2) + pow(q_1new[a1][2], 2) - pow(q_1new[a1][3], 2);
				Req[1][2][a1] = 2 * (q_1new[a1][2] * q_1new[a1][3] - q_1new[a1][0] * q_1new[a1][1]);
				Req[2][0][a1] = 2 * (q_1new[a1][1] * q_1new[a1][3] - q_1new[a1][0] * q_1new[a1][2]);
				Req[2][1][a1] = 2 * (q_1new[a1][0] * q_1new[a1][1] + q_1new[a1][2] * q_1new[a1][3]);
				Req[2][2][a1] = pow(q_1new[a1][0], 2) - pow(q_1new[a1][1], 2) - pow(q_1new[a1][2], 2) + pow(q_1new[a1][3], 2);
				/*cout << Req[0][0][a1] << " "; cout << Req[0][1][a1] << " "; cout << Req[0][2][a1] << endl;
				cout << Req[1][0][a1] << " "; cout << Req[1][1][a1] << " "; cout << Req[1][2][a1] << endl;
				cout << Req[2][0][a1] << " "; cout << Req[2][1][a1] << " "; cout << Req[2][2][a1] << endl; cout <<endl;*/
				Matrix3d  roraionMatrix, R2ed;
				roraionMatrix << Req[0][0][a1], Req[0][1][a1], Req[0][2][a1],
					Req[1][0][a1], Req[1][1][a1], Req[1][2][a1],
					Req[2][0][a1], Req[2][1][a1], Req[2][2][a1];
				R2ed = roraionMatrix * R1;
				if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > -PI / 2 && atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < PI / 2)
				{
					RPY2end[a1][0] = atan2(R2ed(2, 1), R2ed(2, 2)); RPY2end[a1][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[a1][2] = atan2(R2ed(1, 0), R2ed(0, 0));
				}
				if (atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) < -PI / 2 || atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))) > PI / 2)
				{
					RPY2end[a1][0] = atan2(-R2ed(2, 1), -R2ed(2, 2)); RPY2end[a1][1] = atan2(-R2ed(2, 0), sqrt(pow(R2ed(2, 1), 2) + pow(R2ed(2, 2), 2))); RPY2end[a1][2] = atan2(-R2ed(1, 0), -R2ed(0, 0));
				}
				//cout << RPY2end[a1][0] << " "; cout << RPY2end[a1][1] << " "; cout << RPY2end[a1][2] << endl;
				catersianPosture[0] = RPY2end[a1][0];
				catersianPosture[1] = RPY2end[a1][1];
				catersianPosture[2] = RPY2end[a1][2];
				catesianSpacePosture.push_back(catersianPosture);
			}
		}
		//cout << "姿态的数量" << catesianSpacePosture.size() << endl;
		vector<vector<double>>catesianSpacePlan(check, vector<double>(6, 0));
		for (int a1 = 0; a1 <= check-1; a1 = a1 + 1)
		{
			catesianSpacePlan[a1][0] = catesianSpacePosition[a1][0];
			catesianSpacePlan[a1][1] = catesianSpacePosition[a1][1];
			catesianSpacePlan[a1][2] = catesianSpacePosition[a1][2];
			catesianSpacePlan[a1][3] = catesianSpacePosture[a1][0];
			catesianSpacePlan[a1][4] = catesianSpacePosture[a1][1];
			catesianSpacePlan[a1][5] = catesianSpacePosture[a1][2];
		}
		return catesianSpacePlan;
}
	
}

Eigen::Matrix<double, 4, 4> TrajPlanning::RotaToHomo(Eigen::Matrix<double, 3, 3> Rota, Eigen::Matrix<double, 3, 1> Tran)
{
	Eigen::Matrix<double, 4, 4> HomoMatrix;
	HomoMatrix << Rota(0, 0), Rota(0, 1), Rota(0, 2), Tran(0, 0),
		Rota(1, 0), Rota(1, 1), Rota(1, 2), Tran(1, 0),
		Rota(2, 0), Rota(2, 1), Rota(2, 2), Tran(2, 0),
		0, 0, 0, 1;
	return HomoMatrix;
}

vector<double> TrajPlanning::catesianSpacePlanOnLine(vector<double>& positionposture,double state)
{
	vector<double>catesianSpacePlanLine(6);
	if (state == 0)
	{
		catesianSpacePlanLine[0] = positionposture[0] + velocity * interplorationTime * moveDirection_ver[0];
		catesianSpacePlanLine[1] = positionposture[1] + velocity * interplorationTime * moveDirection_ver[1];
		catesianSpacePlanLine[2] = positionposture[2] + velocity * interplorationTime * moveDirection_ver[2];
		catesianSpacePlanLine[3] = positionposture[3];
		catesianSpacePlanLine[4] = positionposture[4];
		catesianSpacePlanLine[5] = positionposture[5];
	}
	if (state == 1)
	{
		catesianSpacePlanLine[0] = positionposture[0];
		catesianSpacePlanLine[1] = positionposture[1];
		catesianSpacePlanLine[2] = positionposture[2];
		catesianSpacePlanLine[3] = positionposture[3];
		catesianSpacePlanLine[4] = positionposture[4];
		catesianSpacePlanLine[5] = positionposture[5];
		state_hold = state_hold + 1;
	}
		return catesianSpacePlanLine;
}